import os
import serial
import re
import math
import sys
import socket
import threading
import time
import json
from datetime import datetime
from PyQt5 import QtCore, QtGui
from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QComboBox, QLineEdit, QListView, QCompleter, QGridLayout, QGraphicsView, QGraphicsScene, QGraphicsLineItem
from PyQt5.QtCore import QThread, pyqtSignal, Qt, QStringListModel, QTime, QTimer, QObject
from PyQt5.QtGui import QStandardItemModel, QStandardItem, QMouseEvent
from widget import Ui_widget_main

from paho.mqtt import client as mqtt_client
import uuid
import pyqtgraph as pg
import AgvParameters
import can
import subprocess
import pandas as pd
import numpy as np
# from pynput import keyboard
from flask import Flask, render_template, request
# region # 定义全局变量
AgvState = {}
virtual_machine = False
controller = "jihecheng"  # RK3568\RK3588
# endregion
# region # 自定义函数及类


def extract_ip():  # 获取本机IP
    st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        st.connect(('10.255.255.255', 1))
        IP = st.getsockname()[0]
    except Exception:
        IP = '127.0.0.1'
    finally:
        st.close()
    return IP


def HexStrAddSpace(HexStr):  # 十六进制字符串标准化打印
    out_data = ''
    for i, s in enumerate(HexStr):
        if ((i % 2 == 0) & (i > 0)):
            out_data += ' '
            out_data += s
        else:
            out_data += s
    return out_data.upper()


def show_text(function):

    def wrapped(self, *args, **kwargs):
        if self.vars["showTextLock"]:
            self.vars["showTextLock"] = False
            result = function(self, *args, **kwargs)
            items = self.get_selected()
            l = len(items)
            l_ = self.vars["listViewModel"].rowCount() - 1
            self.vars["listViewModel"].item(0).setCheckState(
                Qt.Checked if l == l_ else Qt.Unchecked if l == 0 else Qt.PartiallyChecked)
            self.vars["lineEdit"].setText("(全选)" if l == l_ else "(无选择)" if l == 0 else ";".join(
                (item.text() for item in items)))
            self.vars["showTextLock"] = True
        else:
            result = function(self, *args, **kwargs)
        return result

    return wrapped


class QComboCheckBox(QComboBox):

    class MyListView(QListView):

        def __init__(self, parent: QWidget = None, vars=None):
            super().__init__(parent)
            self.vars = vars

        def mousePressEvent(self, event: QMouseEvent):
            self.vars["lock"] = False
            super().mousePressEvent(event)

        def mouseDoubleClickEvent(self, event: QMouseEvent):
            self.vars["lock"] = False
            super().mouseDoubleClickEvent(event)

    def __init__(self, parent: QWidget = None):
        super().__init__(parent)
        self.vars = dict()
        self.vars["lock"] = True
        self.vars["showTextLock"] = True
        # 装饰器锁，避免批量操作时重复改变lineEdit的显示
        self.vars["lineEdit"] = QLineEdit(self)
        self.vars["lineEdit"].setReadOnly(True)
        self.vars["listView"] = self.MyListView(self, self.vars)
        self.vars["listViewModel"] = QStandardItemModel(self)
        self.setModel(self.vars["listViewModel"])
        self.setView(self.vars["listView"])
        self.setLineEdit(self.vars["lineEdit"])

        self.activated.connect(self.__show_selected)

        self.add_item("(全选)")

    def count(self):
        # 返回子项数
        return super().count() - 1

    @show_text
    def add_item(self, text: "str"):
        # 根据文本添加子项
        item = QStandardItem()
        item.setText(text)
        item.setCheckable(True)
        item.setCheckState(Qt.Checked)
        self.vars["listViewModel"].appendRow(item)

    @show_text
    def add_items(self, texts: "tuple or list"):
        # 根据文本列表添加子项
        for text in texts:
            self.add_item(text)

    @show_text
    def clear_items(self):
        # 清空所有子项
        self.vars["listViewModel"].clear()
        self.add_item("(全选)")

    def find_index(self, index: "int"):
        # 根据索引查找子项
        return self.vars["listViewModel"].item(index if index < 0 else index + 1)

    def find_indexs(self, indexs: "tuple or list"):
        # 根据索引列表查找子项
        return [self.find_index(index) for index in indexs]

    def find_text(self, text: "str"):
        # 根据文本查找子项
        tempList = self.vars["listViewModel"].findItems(text)
        tempList.pop(0) if tempList and tempList[0].row() == 0 else tempList
        return tempList

    def find_texts(self, texts: "tuple or list"):
        # 根据文本列表查找子项
        return {text: self.find_text(text) for text in texts}

    def get_text(self, index: "int"):
        # 根据索引返回文本
        return self.vars["listViewModel"].item(index if index < 0 else index + 1).text()

    def get_texts(self, indexs: "tuple or list"):
        # 根据索引列表返回文本
        return [self.get_text(index) for index in indexs]

    def change_text(self, index: "int", text: "str"):
        # 根据索引改变某一子项的文本
        self.vars["listViewModel"].item(
            index if index < 0 else index + 1).setText(text)

    @show_text
    def select_index(self, index: "int", state: "bool" = True):
        # 根据索引选中子项，state=False时为取消选中
        self.vars["listViewModel"].item(
            index if index < 0 else index + 1).setCheckState(Qt.Checked if state else Qt.Unchecked)

    @show_text
    def select_indexs(self, indexs: "tuple or list", state: "bool" = True):
        # 根据索引列表选中子项，state=False时为取消选中
        for index in indexs:
            self.select_index(index, state)

    @show_text
    def select_text(self, text: "str", state: "bool" = True):
        # 根据文本选中子项，state=False时为取消选中
        for item in self.find_text(text):
            item.setCheckState(Qt.Checked if state else Qt.Unchecked)

    @show_text
    def select_texts(self, texts: "tuple or list", state: "bool" = True):
        # 根据文本列表选中子项，state=False时为取消选中
        for text in texts:
            self.select_text(text, state)

    @show_text
    def select_reverse(self):
        # 反转选择
        if self.vars["listViewModel"].item(0).checkState() == Qt.Unchecked:
            self.select_all()
        elif self.vars["listViewModel"].item(0).checkState() == Qt.Checked:
            self.select_clear()
        else:
            for row in range(1, self.vars["listViewModel"].rowCount()):
                self.__select_reverse(row)

    def __select_reverse(self, row: "int"):
        item = self.vars["listViewModel"].item(row)
        item.setCheckState(Qt.Unchecked if item.checkState()
                           == Qt.Checked else Qt.Checked)

    @show_text
    def select_all(self):
        # 全选
        for row in range(0, self.vars["listViewModel"].rowCount()):
            self.vars["listViewModel"].item(row).setCheckState(Qt.Checked)

    @show_text
    def select_clear(self):
        # 全不选
        for row in range(0, self.vars["listViewModel"].rowCount()):
            self.vars["listViewModel"].item(row).setCheckState(Qt.Unchecked)

    @show_text
    def remove_index(self, index: "int"):
        # 根据索引移除子项
        return self.vars["listViewModel"].takeRow(index if index < 0 else index + 1)

    @show_text
    def remove_indexs(self, indexs: "tuple or list"):
        # 根据索引列表移除子项
        return [self.remove_index(index) for index in sorted(indexs, reverse=True)]

    @show_text
    def remove_text(self, text: "str"):
        # 根据文本移除子项
        items = self.find_text(text)
        indexs = [item.row() for item in items]
        return [self.vars["listViewModel"].takeRow(index) for index in sorted(indexs, reverse=True)]

    @show_text
    def remove_texts(self, texts: "tuple or list"):
        # 根据文本列表移除子项
        return {text: self.remove_text(text) for text in texts}

    def get_selected(self):
        # 获取当前选择的子项
        items = list()
        for row in range(1, self.vars["listViewModel"].rowCount()):
            item = self.vars["listViewModel"].item(row)
            if item.checkState() == Qt.Checked:
                items.append(item)
        return items

    def is_all(self):
        # 判断是否是全选
        return True if self.vars["listViewModel"].item(0).checkState() == Qt.Checked else False

    def sort(self, order=Qt.AscendingOrder):
        # 排序，默认正序
        self.vars["listViewModel"].sort(0, order)

    @show_text
    def __show_selected(self, index):
        if not self.vars["lock"]:
            if index == 0:
                if self.vars["listViewModel"].item(0).checkState() == Qt.Checked:
                    self.select_clear()
                else:
                    self.select_all()
            else:
                self.__select_reverse(index)

            self.vars["lock"] = True

    def hidePopup(self):
        if self.vars["lock"]:
            super().hidePopup()


def extract_ip():  # 获取本机IP
    st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        st.connect(('10.255.255.255', 1))
        IP = st.getsockname()[0]
    except Exception:
        IP = '127.0.0.1'
    finally:
        st.close()
    return IP


def get_timestamp():  # 获取当前时间戳
    current_time = datetime.now().strftime('%Y-%m-%dT%H:%M:%S+08:00')
    return current_time


def get_time():  # 获取本机时间
    current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
    return current_time


def update_headerId(data):
    data += 1
    if data > 0xFFFFFFFF:
        data = 0
    return data


def angle_rectify(angle):
    rectify = (angle + 180) % 360 - 180
    return rectify


class thread_testTime(QThread):
    sig_updateAgvPos = pyqtSignal(float, float, float)
    sig_updateAgvSpeed = pyqtSignal(float, float)
    sig_updateMotionDone = pyqtSignal()

    def __init__(self):
        super().__init__()
        self.PGV = PGV()
        self.PGV.start()
        self.PGV.sig_updateOdom.connect(self.updateOdom)

    def run(self):  # 该类模拟后台进程更新时间
        while True:
            time.sleep(0.01)
            print(get_time())

    def updateOdom(self, x, y):
        print(x)


# endregion
# region # 自定义类
class DeviceStatusChecker(QWidget):  # 设备离线监测
    update_deviceStatus = pyqtSignal(str)
    sig_reconnect = pyqtSignal(str)

    def __init__(self):
        super().__init__()
        self.status_handler = 0  # 0: 离线 1: 在线 2: 重连
        self.status_chasis = 0
        self.status_lifter = 0
        self.status_motor = 0

        current_time = QTime.currentTime()
        self.time_handler = current_time
        self.time_chasis = current_time
        self.time_lifter = current_time
        self.time_motor = current_time

        self.heartbeat_timer = QTimer()
        self.heartbeat_timer.timeout.connect(self.check_heartbeat)
        self.heartbeat_timer.start(1000)  # 1秒检测一次

    def check_heartbeat(self):
        current_time = QTime.currentTime()
        interval_time_chasis = self.time_chasis.msecsTo(current_time)
        interval_time_lifter = self.time_lifter.msecsTo(current_time)

        if interval_time_chasis < 3000:
            self.status_chasis = 1
        else:
            self.sig_reconnect.emit('chasis')
            self.status_chasis = 2
            self.time_chasis = current_time
            # print('重连')
        if interval_time_lifter < 3000:
            self.status_lifter = 1
        else:
            self.sig_reconnect.emit('lifter')
            self.status_lifter = 2
            self.time_lifter = current_time


class WebControl(QThread):
    app = Flask(__name__)

    def __init__(self):
        super().__init__()

    def run(self):
        self.app.run(host='0.0.0.0', port=5003, debug=True)

    @app.route('/')
    def index():
        return render_template('index.html')

    @app.route('/stop', methods=['POST'])
    def stop_task():
        # 假设这里有一个任务正在运行，我们停止它
        # 但在这个示例中，我们只是简单地返回一个响应
        print('任务已停止')
        return '任务已停止'


class SignalEmitter(QObject):
    flaskWeb_signal = pyqtSignal(str)  # 假设我们传递一个字符串作为消息


class MotorDrive(QThread):
    signal_send_canData = pyqtSignal(bytes)

    def __init__(self):
        super().__init__()
        # self.activate_can_interface('can0')
        if controller == "jihecheng":
            print("jihecheng")
            # self.bus = can.interface.Bus(bustype='canalystii', channel='can0', bitrate=500000)
        elif os.name == 'posix' and virtual_machine == False:
            self.bus = can.interface.Bus(
                interface='socketcan', channel='can0', bitrate=500000)
        # self.configMotor(0x01)
        self.motorType = "zhongling"  # 中菱电机
        self.scale_rateToLine = 0.618  # 角速度到线速度的转换系数
        self.factor_motorLineSpeed = 100  # 线速度因子
        self.factor_motorRateSpeed = 23.42  # 角速度因子

    def run(self):
        while True:
            time.sleep(1)
            # print(get_time())
            # try:
            #     received_msg = self.bus.recv()
            #     print(f"Received CAN message with device_type {received_msg.arbitration_id} and data {received_msg.data}")
            # except KeyboardInterrupt:
            #     break

    def activate_can_interface(self, interface_name):
        # 构建命令
        command = f"sudo ip link set {interface_name} up type can bitrate 500000"
        # 执行命令
        subprocess.run(command, shell=True, check=True,
                       stdout=subprocess.PIPE, stderr=subprocess.PIPE)
        # result = subprocess.run(command, shell=True, check=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
        # 检查是否有错误输出
        # if result.stderr:
        #     print(f"Error activating CAN interface {interface_name}: {result.stderr.decode()}")
        # else:
        #     print(f"CAN interface {interface_name} activated successfully.")

    def control_motor(self, lineSpeed, rateSpeed):
        leftMotorSpeed = lineSpeed * self.factor_motorLineSpeed + \
            rateSpeed * self.factor_motorRateSpeed
        rightMotorSpeed = lineSpeed * self.factor_motorLineSpeed - \
            rateSpeed * self.factor_motorRateSpeed
        # print(get_time(),lineSpeed)
        # print(get_time())
        speed1 = -int(leftMotorSpeed)  # 左轮安装方向与车体相反，所以取反
        speed2 = int(rightMotorSpeed)
        if self.motorType == "zhongling":
            # 中菱电机
            buf_motor1_speed = [0x23, 0xFF, 0x60, 0x01, speed1 & 0xFF,
                                (speed1 >> 8) & 0xFF, (speed1 >> 16) & 0xFF, (speed1 >> 24) & 0xFF]
            buf_motor2_speed = [0x23, 0xFF, 0x60, 0x02, speed2 & 0xFF,
                                (speed2 >> 8) & 0xFF, (speed2 >> 16) & 0xFF, (speed2 >> 24) & 0xFF]
            frame_id = 0x600 | 0x01
            try:
                self.canSend(frame_id, buf_motor1_speed)
                self.canSend(frame_id, buf_motor2_speed)
            except Exception as e:
                print("Error sending CAN message:", e)
        elif self.motorType == "nideke":
            # 尼德科电机
            buf_motor1_speed = [0x23, 0xFF, 0x60, 0x01, speed1 & 0xFF,
                                (speed1 >> 8) & 0xFF, (speed1 >> 16) & 0xFF, (speed1 >> 24) & 0xFF]
            buf_motor2_speed = [0x23, 0xFF, 0x60, 0x02, speed2 & 0xFF,
                                (speed2 >> 8) & 0xFF, (speed2 >> 16) & 0xFF, (speed2 >> 24) & 0xFF]
            frame_id = 0x600 | 0x01
            try:
                self.canSend(frame_id, buf_motor1_speed)
                self.canSend(frame_id, buf_motor2_speed)
            except Exception as e:
                print("Error sending CAN message:", e)
        else:
            # print("motor_ZL is not defined.")
            pass
            # 其他电机

    def canSend(self, can_id, data):
        msg = can.Message(arbitration_id=can_id,
                          data=data, is_extended_id=False)
        try:
            if controller == "jihecheng":
                self.signal_send_canData.emit(bytes(data))
            elif os.name == 'posix' and virtual_machine == False:
                self.bus.send(msg)
        except Exception as e:
            print("Error sending CAN message:", e)

    def configMotor(self, motorID):
        # 定义数组
        buf_motor1_active = [0x10, 0x10]
        buf_motor1_setMode = [0x2F, 0x60, 0x60, 0x00, 0xfd, 0x00, 0x00, 0x00]
        buf_motor1_clearError = [0x2B, 0x40,
                                 0x60, 0x00, 0x80, 0x00, 0x00, 0x00]
        buf_motor1_prepareServe = [0x2B, 0x40,
                                   0x60, 0x00, 0x06, 0x00, 0x00, 0x00]
        buf_motor1_waitEn = [0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00]
        buf_motor1_en = [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00]
        buf_motor1_setSpeed = [0x23, 0xFF, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00]
        buf_motor1_heartbeat = [0x2B, 0x17, 0x10, 0x00, 0xE8, 0x03, 0x00, 0x00]
        buf_motor1_Watchdog = [0x23, 0x16, 0x10, 0x01, 0x64, 0x00, 0x01, 0x00]
        buf_motor1_protectTime = [0x2B, 0x0C,
                                  0x10, 0x00, 0x64, 0x00, 0x00, 0x00]
        buf_motor1_protectFactor = [0x2F, 0x0D,
                                    0x10, 0x00, 0x01, 0x00, 0x00, 0x00]
        buf_motor_timeoutFactor = [0x2F, 0x2E,
                                   0x20, 0x00, 0x02, 0x00, 0x00, 0x00]

        buf_PDO1_forbid = [0x23, 0x00, 0x18, 0x01, 0x82, 0x01, 0x00, 0x80]
        buf_PDO1_mode = [0x2F, 0x00, 0x18, 0x02, 0xFF, 0x00, 0x00, 0x00]
        buf_PDO1_forbidTime = [0x2B, 0x00, 0x18, 0x03, 0x10, 0x27, 0x00, 0x00]
        buf_PDO1_time = [0x2B, 0x00, 0x18, 0x05, 0xE8, 0x03, 0x00, 0x00]
        buf_PDO1_clear = [0x2F, 0x00, 0x1A, 0x00, 0x00, 0x00, 0x00, 0x00]
        buf_PDO1_address_1 = [0x23, 0x00, 0x1A, 0x01, 0x10, 0x00, 0x01, 0x26]
        buf_PDO1_address_2 = [0x23, 0x00, 0x1A, 0x02, 0x10, 0x00, 0x02, 0x26]
        buf_PDO1_numer = [0x2F, 0x00, 0x1A, 0x00, 0x02, 0x00, 0x00, 0x00]
        buf_PDO1_enable = [0x23, 0x00, 0x18, 0x01, 0x82, 0x01, 0x00, 0x00]

        buf_PDO2_forbid = [0x23, 0x01, 0x18, 0x01, 0x83, 0x02, 0x00, 0x80]
        buf_PDO2_mode = [0x2F, 0x01, 0x18, 0x02, 0xFF, 0x00, 0x00, 0x00]
        buf_PDO2_forbidTime = [0x2B, 0x01, 0x18, 0x03, 0x10, 0x27, 0x00, 0x00]
        buf_PDO2_time = [0x2B, 0x01, 0x18, 0x05, 0xE8, 0x03, 0x00, 0x00]
        buf_PDO2_clear = [0x2F, 0x01, 0x1A, 0x00, 0x00, 0x00, 0x00, 0x00]
        buf_PDO2_address_1 = [0x23, 0x01, 0x1A, 0x01, 0x20, 0x00, 0x63, 0x60]
        buf_PDO2_address_2 = [0x23, 0x01, 0x1A, 0x02, 0x20, 0x00, 0x6C, 0x60]
        buf_PDO2_numer = [0x2F, 0x01, 0x1A, 0x00, 0x02, 0x00, 0x00, 0x00]
        buf_PDO2_enable = [0x23, 0x01, 0x18, 0x01, 0x83, 0x02, 0x00, 0x00]

        frameID = 0x600 | motorID
        DELAY_TIME = 0.02
        time.sleep(DELAY_TIME)

        buf_PDO1_forbid[4] = 0x80 | motorID
        buf_PDO1_enable[4] = 0x80 | motorID
        buf_PDO2_forbid[4] = 0x80 | motorID
        buf_PDO2_enable[4] = 0x80 | motorID

        self.canSend(frameID, buf_motor_timeoutFactor)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_protectTime)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_protectFactor)
        time.sleep(DELAY_TIME)

        self.canSend(frameID, buf_PDO1_forbid)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_mode)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_forbidTime)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_time)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_clear)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_address_1)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_address_2)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_numer)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_enable)
        time.sleep(DELAY_TIME)

        self.canSend(frameID, buf_PDO2_forbid)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_mode)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_forbidTime)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_time)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_clear)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_address_1)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_address_2)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_numer)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_enable)
        time.sleep(DELAY_TIME)

        self.canSend(frameID, buf_motor1_setMode)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_clearError)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_prepareServe)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_waitEn)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_en)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_setSpeed)
        time.sleep(DELAY_TIME)

        print(f"电机{motorID:02x}配置完成;")


class PGV(QThread):
    sig_PGV = pyqtSignal(int, int, int, int)
    sig_updateOdom = pyqtSignal(float, float, float)

    def __init__(self):
        super().__init__()
        # region # 串口初始化
        self.ser = serial.Serial()
        if controller == "jihecheng":
            self.ser.port = '/dev/ttyUSB0'
        else:
            self.ser.port = '/dev/ttyS4'
        # self.ser.port = 'COM8'
        self.ser.baudrate = 115200  # 波特率
        self.ser.timeout = 0.007  # 超时时间（115200）
        self.ser.bytesize = 8  # 数据位
        self.ser.parity = 'N'  # 校验位
        self.ser.stopbits = 1  # 停止位
        self.query = QTimer()
        self.query.timeout.connect(self.fuc_query)
        self.query.start(100)
        self.flag_serial = False
        # endregion
        # region # 二维码数据初始化
        if os.name == 'nt':  # Windows系统
            os.chdir(r'\Code\mqtt_client')
            filename = './config/DmCodes.xlsx'
        elif controller == "jihecheng":
            filename = '/home/robot/Desktop/theyn/mqtt_client/config/DmCodes.xlsx'
        elif virtual_machine == True:  # 虚拟机
            print(os.getcwd())
            print("虚拟机,Linux系统")
            filename = '/home/theyn/Desktop/mqtt_client/config/DmCodes.xlsx'
        elif os.name == 'posix' and virtual_machine == False:  # Linux系统
            print("Linux系统")
            filename = '/home/baer/mqtt_client/config/DmCodes.xlsx'
        df = pd.read_excel(filename)
        array = df.values  # 或者使用 df.to_numpy()
        columns = ['序号', 'tag值', 'X(mm)', 'Y(mm)', '误差角度(°)']
        rows_dict = [dict(zip(columns, row))
                     for row in array]  # 将每行数据转换为字典，并存入列表
        self.dmCodes = {row['tag值']: row for row in rows_dict}  # 创建以tag值为键的字典
        # endregion

    def run(self):
        # try:
        if controller == "jihecheng":
            try:
                self.ser.open()
                self.flag_serial = True
            except serial.SerialException:
                print('相机未连接')
        elif os.name == 'posix' and virtual_machine == False:
            self.ser.open()
            self.flag_serial = True
        # except Exception as e:
        #     print(e)
        #     print('此串口不能正常打开！')
        #     self.flag_serial = False
        while True:
            # while False:
            try:
                if self.ser.in_waiting:
                    data = self.ser.readall()  # 不能接收持续发送的数据
                    if not data or data == 'exit':
                        print("break")
                        break
                    # self.signal.data_process.emit(data)
                    # print(len(data))
                    if len(data) == 21:
                        self.analysisPGV(data)
            except Exception:
                if self.flag_serial:
                    print("串口接收数据异常")
                self.flag_serial = False
                time.sleep(1)
                try:
                    if controller == "jihecheng":
                        self.ser.open()
                        self.flag_serial = True
                    elif os.name == 'posix' and virtual_machine == False:
                        self.ser.open()
                        self.flag_serial = True
                except serial.SerialException:
                    # print('相机串口连接异常！')
                    pass

    def analysisPGV(self, i_PGV_Data):
        # 初始化变量
        flag_tagChange = 0
        tmp_posX = 0
        tmp_tagNum = 0
        tmp_posY = 0
        tmp_posYaw = 0
        tmp_agv_imuAngle = 0.0

        # posX
        if i_PGV_Data[2] & 0x04:
            # posX是负数
            tmp_posX = (0xffffff - ((i_PGV_Data[5] & 0x7f) | ((i_PGV_Data[4] & 0x7f) << 7) | ((i_PGV_Data[3] & 0x7f) << 14) |
                                    ((i_PGV_Data[2] & 0x07) << 21))) * -1
        else:
            # posX是正数
            tmp_posX = (i_PGV_Data[5] & 0x7f) | ((i_PGV_Data[4] & 0x7f) << 7) | (
                (i_PGV_Data[3] & 0x7f) << 14) | ((i_PGV_Data[2] & 0x03) << 21)

        # posY
        if i_PGV_Data[6] & 0x40:
            # posY是负数
            tmp_posY = (
                (~((i_PGV_Data[7] & 0x7f) | ((i_PGV_Data[6] & 0x7f) << 7)) + 1) & 0x3fff) * -1
        else:
            # posY是正数
            tmp_posY = (i_PGV_Data[7] & 0x7f) | ((i_PGV_Data[6] & 0x7f) << 7)

        # posYAW
        if i_PGV_Data[10] & 0x40:
            # posYAW是负数
            tmp_posYaw = ((~((i_PGV_Data[11] & 0x7f) | (
                (i_PGV_Data[10] & 0x7f) << 7)) + 1) & 0x3fff) * -1
        else:
            # posYAW是正数
            tmp_posYaw = (i_PGV_Data[11] & 0x7f) | (
                (i_PGV_Data[10] & 0x7f) << 7)

        tmp_posYaw = 3600 - tmp_posYaw
        tmp_tagNum = (i_PGV_Data[17] & 0x7f) | ((i_PGV_Data[16] & 0x7f) << 7) | (
            (i_PGV_Data[15] & 0x7f) << 14) | ((i_PGV_Data[14] & 0x7f) << 21)

        # 打印调试信息
        # print(f"X位置: {tmp_posX}, Y位置: {tmp_posY}, 角度: {tmp_posYaw}, 编码: {tmp_tagNum}")
        self.sig_PGV.emit(tmp_tagNum, tmp_posX, tmp_posY, tmp_posYaw)
        if tmp_tagNum:
            matched_data = self.dmCodes.get(tmp_tagNum)  # 匹配tag值
            if matched_data:
                posX = (matched_data['X(mm)'] + tmp_posX / 10) / 1000
                posY = (matched_data['Y(mm)'] + tmp_posY / 10) / 1000
                posYaw = (matched_data['误差角度(°)'] + tmp_posYaw / 10)
                # print(f"X位置: {posX}, Y位置: {posY}, 角度: {tmp_posYaw}, 编码: {tmp_tagNum}")
                self.sig_updateOdom.emit(posX, posY, posYaw)
            else:
                print("匹配失败")

    def fuc_query(self):
        buf_PGV_query = bytes([0xC8, 0x37])
        if self.flag_serial:
            self.ser.write(buf_PGV_query)


class Odometer(QThread):
    sig_updateAgvPos = pyqtSignal(float, float, float)
    sig_updateAgvSpeed = pyqtSignal(float, float)
    sig_updateMotionDone = pyqtSignal()

    def __init__(self):
        super().__init__()
        # region # 里程计相关
        self.agv_posX = AgvState['agvPosition']['x']
        self.agv_posY = AgvState['agvPosition']['y']
        self.agv_theta = AgvState['agvPosition']['theta']
        self.lineSpeed = 0
        self.rateSpeed = 0
        self.multiLineSpeed = 1  # 线速度倍率(提高模拟效率)
        self.multiRateSpeed = 1  # 角速度倍率(提高模拟效率)
        self.start_time = time.time()
        # self.execution_time = 0
        self.mode_motion = 'stop'  # 'stop' 'move' 'moveToNode' 'rotate' 'rotateToNode'
        self.startNode_x = 0  # 起始点x
        self.startNode_y = 0  # 起始点y
        self.targetNode_x = 0  # 目标点x
        self.targetNode_y = 0  # 目标点y
        self.planDistance_last = 0  # 上次距离目标点距离
        self.decDistance = 0  # 提前减速距离
        self.flag_slowDown = False  # 是否开始减速
        self.flag_enSlowDown = True  # 是否需要减速
        self.creepLineSpeed = 0.03  # 爬行线速度
        self.creepDistance = 0.03  # 爬行距离

        self.rotateTheta_T = 0  # 目标角度
        self.planTheta_last = 0  # 上次距离目标点角度
        self.decTheta = 0  # 提前减速角度
        self.creepRateSpeed = 0.03  # 爬行角速度
        self.creepTheta = 0.03  # 爬行角度

        # endregion
        # region # 速度曲线相关
        self.flag_controlSpeed = 0
        self.lineSpeed_T = 0
        self.lineSpeed_C = 0
        self.agv_max_lineSpeed = 1.2
        self.factor_sysDelay = 1  # python代码执行效率低，定时10ms循环执行只能到30-50ms，提高AGV模拟器加减速度
        self.agv_acceleration = 0.7 * self.multiLineSpeed * self.factor_sysDelay
        self.agv_deceleration = 0.7 * self.multiLineSpeed * self.factor_sysDelay
        self.rateSpeed_T = 0
        self.rateSpeed_C = 0
        self.agv_max_rateSpeed = 1.2
        self.agv_RateAcceleration = 0.7 * self.multiRateSpeed * self.factor_sysDelay
        self.agv_RateDeceleration = 0.7 * self.multiRateSpeed * self.factor_sysDelay
        self.time_period = 20
        self.time_interval = self.time_period / 1000
        # endregion

        # region # 相机修正
        # self.PGV = PGV()
        # self.PGV.start()
        # self.PGV.sig_updateOdom.connect(self.updateOdom)
        # endregion
        # region # standly纠偏
        self.K_Cross = 3
        # 横向误差系数
        self.K_LowVelo = 0.5
        # 低速阻尼
        self.K_trace_angle = 3
        # 航向角纠偏系数
        self.K_cross_angle = 1
        # 横向距离纠偏系数
        self.path_direction = 0
        # 路径方向
        # endregion

    def run(self):
        while True:
            start_time = time.time()
            # region # 里程计相关
            if self.rateSpeed != 0 or self.lineSpeed != 0:
                # region # 更新AGV位置
                if self.rateSpeed != 0:
                    div_rateSpeed = self.rateSpeed * self.time_interval * self.multiRateSpeed
                    self.agv_theta += div_rateSpeed
                    # 确保偏航角差在[-pi, pi]范围内
                    self.agv_theta = (
                        self.agv_theta + math.pi) % (2 * math.pi) - math.pi
                if self.lineSpeed != 0:
                    div_lineSpeed = self.lineSpeed * self.time_interval * self.multiLineSpeed
                    self.agv_posX += div_lineSpeed * math.cos(self.agv_theta)
                    self.agv_posY += div_lineSpeed * math.sin(self.agv_theta)
                self.sig_updateAgvPos.emit(
                    self.agv_posX, self.agv_posY, self.agv_theta)
                # endregion
                if self.mode_motion == 'moveToNode':
                    x1 = self.agv_posX
                    y1 = self.agv_posY
                    x2 = self.targetNode_x
                    y2 = self.targetNode_y
                    x3 = self.startNode_x
                    y3 = self.startNode_y
                    A = (x2, y2)
                    B = (x1, y1)
                    C = (x3, y3)

                    # 横向距离
                    distance_direction, distance_cross = self.projection_distance(
                        A, B, C)  # 投影距离,横向距离
                    print(distance_direction)
                    # 横向纠偏角
                    if distance_direction < 0.05:
                        angle_cross = 0
                    else:
                        angle_cross = math.atan2(
                            self.K_Cross * distance_cross, abs(self.lineSpeed) + self.K_LowVelo)
                    # 航向角
                    car_degrees = self.agv_theta * (180 / math.pi)
                    car_degrees = angle_rectify(car_degrees)
                    # 航向角偏差
                    trace_angle_deviation = self.path_direction - car_degrees
                    trace_angle_deviation = angle_rectify(
                        trace_angle_deviation)
                    # 纠偏角速度
                    self.rateSpeed = self.K_trace_angle * trace_angle_deviation / \
                        180 * math.pi + self.K_cross_angle * angle_cross
                    # if self.rateSpeed > 0:
                    #     print(self.rateSpeed)
                    self.flag_controlSpeed = 1

                    # 到达目标点
                    if distance_direction < 0.005:
                        print(
                            f'到达目标点,本次距离{distance_direction:.3f}m,当前速度{self.lineSpeed:.3f}m/s')
                        if self.flag_enSlowDown:
                            self.lineSpeed_T = 0
                            self.rateSpeed_T = 0
                            self.flag_controlSpeed = 1
                        self.sig_updateMotionDone.emit()
                        self.mode_motion = 'stop'
                        self.flag_slowDown = False
                    # 接近目标点
                    elif distance_direction < self.decDistance and self.flag_slowDown == False and self.flag_enSlowDown:
                        print(f'开始减速{distance_direction:.3f}m')
                        self.lineSpeed_T = self.creepLineSpeed
                        self.rateSpeed_T = 0
                        self.flag_controlSpeed = 1
                        self.flag_slowDown = True
                    # # 行进纠偏
                    # else:
                    #     self.rotateTheta_T = self.calculate_theta(x1, y1, x2, y2)
                    #     planTheta = self.calculate_theta_deviation(self.agv_theta, self.rotateTheta_T)
                    #     self.rateSpeed_T = self.lineSpeed * math.sin(planTheta) / distance_direction
                    #     self.flag_controlSpeed = 1
                elif self.mode_motion == 'rotateToNode':
                    planTheta = abs(self.calculate_theta_deviation(
                        self.agv_theta, self.rotateTheta_T))
                    # 到达目标角
                    if planTheta < 0.005 or planTheta - self.planTheta_last > 0.001:
                        if planTheta < 0.005:
                            print(
                                f'到达目标角度,本次偏差弧度{planTheta:.3f},当前速度{self.rateSpeed:.3f}')
                        else:
                            print(
                                f'到达目标角度,本次偏差角度{planTheta:.3f},上次偏差角度{self.planTheta_last:.3f}m')
                        self.lineSpeed_T = 0
                        self.rateSpeed_T = 0
                        self.flag_controlSpeed = 1
                        self.sig_updateMotionDone.emit()
                        self.mode_motion = 'stop'
                        self.flag_slowDown = False
                    # 接近目标角
                    elif planTheta < self.decTheta and self.flag_slowDown == False:
                        print(f'开始减速{planTheta:.3f}')
                        self.lineSpeed_T = 0
                        if self.rateSpeed >= 0:
                            self.rateSpeed_T = self.creepRateSpeed
                        else:
                            self.rateSpeed_T = -self.creepRateSpeed
                        self.flag_controlSpeed = 1
                        self.flag_slowDown = True
                    self.planTheta_last = planTheta
            # endregion
            # region # 速度曲线相关
            if self.flag_controlSpeed:
                # region # 线速度曲线控制
                self.lineSpeed_T = max(
                    min(self.lineSpeed_T, self.agv_max_lineSpeed), -self.agv_max_lineSpeed)
                if self.lineSpeed_C == self.lineSpeed_T:
                    pass
                elif (self.lineSpeed_C < self.lineSpeed_T) and (self.lineSpeed_C >= 0) and (self.lineSpeed_T >= 0):
                    # 当前速度和目标速度同正,正向加速
                    self.lineSpeed_C += self.agv_acceleration * self.time_period / 1000
                    if self.lineSpeed_C > self.lineSpeed_T:
                        self.lineSpeed_C = self.lineSpeed_T
                elif (self.lineSpeed_C > self.lineSpeed_T) and (self.lineSpeed_C >= 0) and (self.lineSpeed_T >= 0):
                    # 当前速度和目标速度同正,正向减速
                    self.lineSpeed_C -= self.agv_deceleration * self.time_period / 1000
                    if self.lineSpeed_C < self.lineSpeed_T:
                        self.lineSpeed_C = self.lineSpeed_T
                elif (self.lineSpeed_C > self.lineSpeed_T) and (self.lineSpeed_C <= 0) and (self.lineSpeed_T <= 0):
                    # 当前速度和目标速度同负,反向加速
                    self.lineSpeed_C -= self.agv_acceleration * self.time_period / 1000
                    if self.lineSpeed_C < self.lineSpeed_T:
                        self.lineSpeed_C = self.lineSpeed_T
                elif (self.lineSpeed_C < self.lineSpeed_T) and (self.lineSpeed_C <= 0) and (self.lineSpeed_T <= 0):
                    # 当前速度和目标速度同负,反向减速
                    self.lineSpeed_C += self.agv_deceleration * self.time_period / 1000
                    if self.lineSpeed_C > self.lineSpeed_T:
                        self.lineSpeed_C = self.lineSpeed_T
                elif (self.lineSpeed_C < 0) and (self.lineSpeed_T > 0):
                    # 当前速度负，目标速度正,先反向减速
                    self.lineSpeed_C += self.agv_deceleration * self.time_period / 1000
                elif (self.lineSpeed_C > 0) and (self.lineSpeed_T < 0):
                    # 当前速度正，目标速度负,先正向减速
                    self.lineSpeed_C -= self.agv_deceleration * self.time_period / 1000
                else:
                    print("线速度数据错误;")
                # endregion
                # region # 角速度曲线控制
                self.rateSpeed_T = max(
                    min(self.rateSpeed_T, self.agv_max_rateSpeed), -self.agv_max_rateSpeed)
                if self.rateSpeed_C == self.rateSpeed_T:
                    pass
                elif (self.rateSpeed_C < self.rateSpeed_T) and (self.rateSpeed_C >= 0) and (self.rateSpeed_T >= 0):
                    # 当前速度和目标速度同正,正向加速
                    self.rateSpeed_C += self.agv_RateAcceleration * self.time_period / 1000
                    if self.rateSpeed_C > self.rateSpeed_T:
                        self.rateSpeed_C = self.rateSpeed_T
                elif (self.rateSpeed_C > self.rateSpeed_T) and (self.rateSpeed_C >= 0) and (self.rateSpeed_T >= 0):
                    # 当前速度和目标速度同正,正向减速
                    self.rateSpeed_C -= self.agv_RateDeceleration * self.time_period / 1000
                    if self.rateSpeed_C < self.rateSpeed_T:
                        self.rateSpeed_C = self.rateSpeed_T
                elif (self.rateSpeed_C > self.rateSpeed_T) and (self.rateSpeed_C <= 0) and (self.rateSpeed_T <= 0):
                    # 当前速度和目标速度同负,反向加速
                    self.rateSpeed_C -= self.agv_RateAcceleration * self.time_period / 1000
                    if self.rateSpeed_C < self.rateSpeed_T:
                        self.rateSpeed_C = self.rateSpeed_T
                elif (self.rateSpeed_C < self.rateSpeed_T) and (self.rateSpeed_C <= 0) and (self.rateSpeed_T <= 0):
                    # 当前速度和目标速度同负,反向减速
                    self.rateSpeed_C += self.agv_RateDeceleration * self.time_period / 1000
                    if self.rateSpeed_C > self.rateSpeed_T:
                        self.rateSpeed_C = self.rateSpeed_T
                elif (self.rateSpeed_C < 0) and (self.rateSpeed_T > 0):
                    # 当前速度负，目标速度正,先反向减速
                    self.rateSpeed_C += self.agv_RateDeceleration * self.time_period / 1000
                elif (self.rateSpeed_C > 0) and (self.rateSpeed_T < 0):
                    # 当前速度正，目标速度负,先正向减速
                    self.rateSpeed_C -= self.agv_RateDeceleration * self.time_period / 1000
                else:
                    print("角速度数据错误;")
                # endregion
                if self.lineSpeed_C == self.lineSpeed_T and self.rateSpeed_C == self.rateSpeed_T:
                    self.flag_controlSpeed = 0
                self.lineSpeed_C = round(self.lineSpeed_C, 3)
                self.rateSpeed_C = round(self.rateSpeed_C, 3)
                # print(self.execution_time,self.lineSpeed_C)
                # print(start_time)
                # print("线速度:", self.lineSpeed_C, "角速度:", self.rateSpeed_C)
                self.sig_updateAgvSpeed.emit(
                    self.lineSpeed_C, self.rateSpeed_C)
                self.lineSpeed = self.lineSpeed_C
                self.rateSpeed = self.rateSpeed_C
                # print(get_time())
            # endregion
            end_time = time.time()
            execution_time = end_time - start_time
            sleep_time = self.time_interval - 0.001 - \
                execution_time  # 理论time_interval 实际测试-0.001
            if sleep_time > 0:
                time.sleep(sleep_time)
            # print(self.execution_time)

    def projection_distance(self, A, B, C):  # 目标A，当前B，起点C；计算AB在AC的投影长度
        # 提取点的坐标
        xA, yA = A
        xB, yB = B
        xC, yC = C

        # 计算向量AB和AC
        AB = (xB - xA, yB - yA)
        AC = (xC - xA, yC - yA)

        # 计算点积
        dot_product = AB[0] * AC[0] + AB[1] * AC[1]

        # 计算向量AC的模
        magnitude_AC = math.sqrt(AC[0]**2 + AC[1]**2)

        # 计算叉积
        cross_product = AB[0] * AC[1] - AB[1] * AC[0]

        # 计算向量AC的模
        magnitude_AC = math.sqrt(AC[0]**2 + AC[1]**2)

        # 计算投影距离
        projection_dist = dot_product / magnitude_AC

        # 计算点B到向量AC的距离
        cross_distance = cross_product / magnitude_AC

        return projection_dist, cross_distance

    def calculate_distance(self, x1, y1, x2, y2):
        distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
        return distance

    def calculate_theta(self, x1, y1, x2, y2):
        theta = math.atan2((y2 - y1), (x2 - x1))
        return theta

    def calculate_theta_deviation(self, theta_C, theta_T):
        theta = theta_T - theta_C
        theta = (theta + math.pi) % (2 * math.pi) - math.pi
        return theta

    def updateOdom(self, x, y, angle):
        self.agv_posX = x
        self.agv_posY = y
        if angle > 180:
            angle -= 360
        elif angle < -180:
            angle += 360
        self.agv_theta = angle / 180 * math.pi


class MqttThread(QThread):
    sig_connect_feedback = pyqtSignal(bool)
    server_log = pyqtSignal(str)
    sig_data_analysis = pyqtSignal(str, bytes)
    signal_send_data = pyqtSignal(bytes)
    received_data = pyqtSignal(tuple, str)
    update_clients_list = pyqtSignal(str, tuple)
    flaskWeb_signal = pyqtSignal()
    sig_controlDevice_change = pyqtSignal(str)

    def __init__(self, ip, port, device_type):
        super().__init__()
        self.ip = ip
        self.port = port
        self.device_type = device_type
        self.client_sockets = []
        self.controlDevice = ''
        self.signal_send_data.connect(self.send_data)
        self.sig_controlDevice_change.connect(self.controlDevice_change)

    def run(self):
        self.client = mqtt_client.Client(
            mqtt_client.CallbackAPIVersion.VERSION2, self.device_type)
        # print('self.device_type:',self.device_type)
        # self.client = mqtt_client.Client(str(uuid.uuid4()))
        self.client.on_connect = self.on_connect  # 返回连接状态的回调函数
        self.client.on_message = self.on_message  # 返回订阅消息回调函数
        self.client.username_pw_set("theyn", "password")  # mqtt服务器账号密码
        try:
            self.client.connect(self.ip, self.port, 60)  # 连接mqtt服务器
            self.client.loop_start()
            self.flag_run = True
            self.server_log.emit("已连接MQTT服务器")
            self.sig_connect_feedback.emit(True)
            while self.flag_run:
                time.sleep(1)
                # print("服务器正在运行...")
            self.client.disconnect()
            self.server_log.emit("已断开MQTT服务器")
            print("服务器已停止")
        except Exception as e:
            self.server_log.emit("连接失败" + str(e))
            self.sig_connect_feedback.emit(False)

    def stop(self):
        self.flag_run = False

    def send_data(self, data):
        for i in range(0, len(self.client_sockets)):
            try:
                self.client_sockets[i].send(data)
            except:
                self.client_sockets.remove(self.client_sockets[i])
                self.server_log.emit(f"客户端 {self.client_sockets[i]} 已断开连接")

    def handle_client(self, client_socket, client_address):
        client_text = client_address[0] + ':' + str(client_address[1])
        print(f"客户端 {client_text} 已连接")
        while self.flag_run:
            try:
                data = client_socket.recv(1024)
                if not data:
                    break
                if (client_text == self.controlDevice):  # 当前控制设备
                    self.server_log.emit(data)
                else:  # 其他设备
                    try:
                        self.received_data.emit(
                            client_address, data.decode('utf-8'))
                    except Exception as e:
                        print(e)
            except Exception as e:
                print(f"客户端 {client_address} 断开连接，原因： {e}")
                break
        client_socket.close()
        self.server_log.emit(f"客户端 {client_address} 断开连接")
        self.client_sockets.remove(client_socket)
        self.update_clients_list.emit("remove", client_address)  # 更新界面列表

    def controlDevice_change(self, device):
        self.controlDevice = device
        print(self.controlDevice)

    def on_connect(self, client, userdata, flags, reason_code, properties):
        print(f"Connected with result code {reason_code}")
        # 在连接时设置遗嘱
        # 主题（遗嘱将被发送到此主题）
        TOPIC = "Test/Topic1"
        # 遗嘱消息
        lastWill = AgvParameters.lastWill
        lastWill['timestamp'] = get_timestamp()
        LWT_MESSAGE = json.dumps(lastWill)
        # LWT_MESSAGE = f"Client disconnected unexpectedly"
        # 遗嘱的QoS
        LWT_QOS = 1
        # 遗嘱的retain标志
        LWT_RETAIN = True
        self.client.will_set(TOPIC, LWT_MESSAGE, LWT_QOS, LWT_RETAIN)

    def on_message(self, client, userdata, msg):
        # print(f"{msg.topic} {msg.payload}")
        message = msg.topic + ': ' + msg.payload.decode("utf-8")
        self.server_log.emit(message)
        self.sig_data_analysis.emit(msg.topic, msg.payload)

    def subscribe(self, topic):
        self.client.subscribe(topic)
        self.server_log.emit('subscribed to topic ' + topic)

    def unsubscribe(self, topic):
        self.client.unsubscribe(topic)
        self.server_log.emit('unsubscribed from topic ' + topic)

    def publish(self, topic, message, isShow):
        self.client.publish(topic, message)
        if isShow:
            self.server_log.emit('published to topic ' +
                                 topic + ' message ' + message)


class ServerThread(QThread):
    received_data = pyqtSignal(tuple, str)
    server_log = pyqtSignal(str)
    update_clients_list = pyqtSignal(str, tuple)
    stop_signal = pyqtSignal()
    signal_send_data = pyqtSignal(bytes)
    sig_data_process = pyqtSignal(bytes)
    sig_controlDevice_change = pyqtSignal(str)

    def __init__(self, host, port):
        super().__init__()
        self.host = host
        self.port = port
        self.client_sockets = []
        self.controlDevice = ''
        self.signal_send_data.connect(self.send_data)
        self.sig_controlDevice_change.connect(self.controlDevice_change)

    def run(self):
        self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.server_socket.bind((self.host, self.port))
        self.server_socket.listen(5)
        self.flag_run = True
        self.server_log.emit("服务器已启动，等待客户端连接...")

        while self.flag_run:
            try:
                client_socket, client_address = self.server_socket.accept()
                # client_socket.settimeout(2)  # 设置超时时间为3秒,3s内无数据传输则断开连接
                self.server_log.emit(f"客户端 {client_address} 已连接")
                self.client_sockets.append(client_socket)
                self.update_clients_list.emit("add", client_address)  # 更新界面列表
                threading.Thread(target=self.handle_client, args=(
                    client_socket, client_address)).start()
            except:
                self.server_log.emit("服务器已关闭")
                # client_socket.close()

    def stop(self):
        self.flag_run = False
        self.server_socket.close()
        # 关闭所有连接设备
        # for i in range(0,len(self.client_sockets)):
        #     try:
        #         self.client_sockets[i].send(b"exit")
        #         self.client_sockets[i].close()
        #     except:
        #         break
        # self.client_sockets=[]

    def send_data(self, data):
        for i in range(0, len(self.client_sockets)):
            try:
                self.client_sockets[i].send(data)
            except:
                self.client_sockets.remove(self.client_sockets[i])
                self.server_log.emit(f"客户端 {self.client_sockets[i]} 已断开连接")

    def handle_client(self, client_socket, client_address):
        client_text = client_address[0] + ':' + str(client_address[1])
        while self.flag_run:
            try:
                data = client_socket.recv(1024)
                if not data:
                    break
                # if (client_text == self.controlDevice):  #当前控制设备
                if (client_text == client_text):  # 当前控制设备
                    self.sig_data_process.emit(data)
                else:  # 其他设备
                    try:
                        print(client_text, data.decode('utf-8'))
                        self.received_data.emit(
                            client_address, data.decode('utf-8'))
                    except Exception as e:
                        print(e)
            except Exception as e:
                print(f"客户端 {client_address} 断开连接，原因： {e}")
                break
        client_socket.close()
        self.server_log.emit(f"客户端 {client_address} 断开连接")
        self.client_sockets.remove(client_socket)
        self.update_clients_list.emit("remove", client_address)  # 更新界面列表

    def controlDevice_change(self, device):
        self.controlDevice = device
        print(self.controlDevice)


class clientThread(QThread):
    # 定义信号来发送接收到的数据
    data_received = pyqtSignal(tuple, int, bytes)
    log = pyqtSignal(str)
    disConnect = pyqtSignal(int, QThread)

    def __init__(self, device_type, server_address):
        super().__init__()
        self.device_type = device_type
        self.server_address = server_address
        try:
            self.identify = server_address.getpeername()
            self.running = True
        except:
            self.identify = server_address
            self.running = False

    def run(self):
        current_thread = threading.current_thread()  # 获取线程对象、名称，便于从线程列表中移除
        sender = self.sender()
        # print(f"Thread {current_thread.name} started")
        # print(sender)
        # print(f"Running in thread: {self}")
        # print(self.device_type)
        while self.running:
            try:
                data = self.server_address.recv(1024)
                if data:
                    self.data_received.emit(
                        self.identify, self.device_type, data)
                else:
                    self.stop()
            except:
                self.stop()

    def stop(self):
        self.log.emit(str(self.identify) + '断开连接')
        self.server_address.close()
        self.running = False

    def send_message(self, message):
        if self.server_address and self.server_address.fileno() != -1:
            try:
                self.server_address.sendall(message)
            except:
                self.stop()
                # self.log.emit(f"Error sending message to {str(self.identify)}")
                # self.log.emit(str(self.identify) + "已断开连接")
                # self.disConnect.emit(self.device_type, self)
                # self.server_address.close()
        else:
            self.log.emit(str(self.identify) + "连接不存在")

            # self.disConnect.emit(self.server_address)


class SerialPort(QThread):
    data_received = pyqtSignal(tuple, int, bytes)

    def __init__(self, port, baudrate=9600, timeout=1, device_type=0):
        """
        初始化串口对象

        :param port: 串口名称，例如 'COM3' 或 '/dev/ttyUSB0'
        :param baudrate: 波特率，默认 9600
        :param timeout: 读取超时时间，默认 1 秒
        """
        super().__init__()
        self.port = port
        self.baudrate = baudrate
        self.timeout = timeout
        self.serial = None
        self.identify = port
        self.device_type = device_type

    def run(self):
        self.open()
        while self.is_open():
            try:
                if self.serial.in_waiting:
                    data = self.serial.read_all()
                    if not data or data == 'exit':
                        print("exit")
                        self.close()
                        break
                    else:
                        self.data_received.emit(
                            self.identify, self.device_type, data)
            except Exception as e:
                print(f"串口读取错误: {e}")
                self.close()
                break

    def open(self):
        """打开串口"""
        try:
            self.serial = serial.Serial(
                self.port, self.baudrate, timeout=self.timeout)
            print(f"串口 {self.port} 已打开")
        except serial.SerialException as e:
            print(f"无法打开串口 {self.port}: {e}")

    def is_open(self):
        """检查串口是否已打开"""
        return self.serial and self.serial.is_open

    def close(self):
        """关闭串口"""
        if self.serial and self.serial.is_open:
            self.serial.close()
            print(f"串口 {self.port} 已关闭")
        else:
            print(f"串口 {self.port} 未打开")

    def write(self, data):
        """
        写入数据到串口

        :param data: 要写入的数据，字符串或字节
        """
        if not self.is_open():
            print("串口未打开")
            return

        if isinstance(data, str):
            data = data.encode('utf-8')  # 将字符串转换为字节
        try:
            self.serial.write(data)
        except serial.SerialException as e:
            print(f"写入数据时出错: {e}")


class ScatterPlot(pg.ScatterPlotItem):
    sig_ptData = pyqtSignal(dict, dict)

    def __init__(self, *args, **kargs):
        super().__init__(*args, **kargs)

    def mouseClickEvent(self, ev):
        if ev.button() == QtCore.Qt.MouseButton.LeftButton:
            pts = self.pointsAt(ev.pos())
            if len(pts) > 0:
                # print(pts[0])
                dic2 = {}
                if len(pts) > 1:
                    dic2 = pts[1].data()
                self.sig_ptData.emit(pts[0].data(), dic2)
                self.ptsClicked = pts
                ev.accept()
                self.sigClicked.emit(self, self.ptsClicked, ev)
            else:
                print("no spots")
                ev.ignore()
        else:
            ev.ignore()


class LineSegmentROI(pg.LineSegmentROI):
    sig_lineData = pyqtSignal(dict)
    color = (0, 0, 255, 155)
    hovercolor = (255, 0, 0, 100)
    pen = pg.mkPen(color=color, width=7)
    hoverPen = pg.mkPen(color=hovercolor, width=7)
    handlePen = pg.mkPen(color=color, width=7)
    handleHoverPen = pg.mkPen(color=hovercolor, width=7)
    sig_removeSelf = pyqtSignal(object)

    def __init__(self,
                 data='',
                 pen=pen,
                 hoverPen=hoverPen,
                 handleHoverPen=handleHoverPen,
                 handlePen=handlePen,
                 movable=True,
                 rotatable=True,
                 resizable=True,
                 removable=False,
                 *args,
                 **kargs):

        super().__init__(movable=movable, pen=pen, hoverPen=hoverPen,
                         handleHoverPen=handleHoverPen, handlePen=handlePen, *args, **kargs)
        self.data = data

    def mouseClickEvent(self, ev):
        if ev.button() == QtCore.Qt.MouseButton.LeftButton:
            # print(self.data)
            self.sig_lineData.emit(self.data)
            # self.sig_removeSelf.emit(self)

    # def mouseDragEvent(self, ev):
    #     pass
    def movePoint(self, handle, pos, modifiers=None, finish=True, coords='parent'):
        print("movePoint")
        pass


class PaintRoute(pg.PlotWidget):  # 运动轨迹绘制
    sig_lineData = pyqtSignal(dict)

    def __init__(self, title="AGV 运动轨迹", background='w', parent=None):
        super().__init__(parent, title=title, background=background)
        self.lines = []  # 存储所有线段的列表
        # 设置图表
        self.setXRange(-10, 10)
        self.setYRange(-10, 10)
        self.showGrid(x=True, y=True, alpha=0.5)

        self.dotdata = []
        color = (0, 0, 255, 100)
        self.scatter = ScatterPlot(
            size=10,
            brush=pg.mkBrush(color),
            pen=pg.mkPen(color),
            # pxMode=False,
            hoverable=True,
            hoverPen=pg.mkPen('b'),
            hoverBrush=pg.mkBrush('b'),
            hoverSize=10)
        self.addItem(self.scatter)

        # self.scene().sigMouseClicked.connect(self.on_mouse_click)
        # AGV车身位置
        color = (0, 255, 0, 100)
        self.agvModel = pg.ScatterPlotItem(size=20, symbol='o', pen=pg.mkPen(
            color=color), brush=pg.mkBrush(color=color))
        self.addItem(self.agvModel)
        x = AgvParameters.agvState['agvPosition']['x']
        y = AgvParameters.agvState['agvPosition']['y']
        self.agvPos = [{'pos': (x, y)}]
        self.agvModel.setData(self.agvPos)
        # AGV运动轨迹
        color = (100, 100, 100, 100)
        self.agvTrajectory = pg.ScatterPlotItem(
            size=10, symbol='o', pen=pg.mkPen(color=color), brush=pg.mkBrush(color=color))
        self.addItem(self.agvTrajectory)
        self.agvTrajectoryDot = []

    def on_mouse_click(self, event):
        # 这里我们只是打印消息，而不是删除线条
        # 实际上，你需要实现一些逻辑来确定点击是否在线条上
        print(f"Mouse clicked at position: {event.pos()}")

    def refresh_dot_agvTrajectoryDot(self):
        self.agvTrajectory.setData(self.agvTrajectoryDot)

    def refresh_dot_order(self):
        self.scatter.setData(self.dotdata)

    def show_lineData(self, data):
        self.sig_lineData.emit(data)

    def remove_line(self, line):
        self.removeItem(line)

    def remove_last_line(self):
        if self.lines:
            last_line = self.lines.pop()  # 从列表中移除最后一个线段
            self.removeItem(last_line)  # 从图表中移除线段


# endregion


class Window_main(QMainWindow, Ui_widget_main.Ui_MainWindow):  # 主窗口
    sig_print = pyqtSignal(str, str, bool)
    app = Flask(__name__)

    def __init__(self):
        # region # 初始化
        super().__init__()
        self.setupUi(self)
        self.setFocusPolicy(Qt.FocusPolicy.StrongFocus)  # 设置获取焦点的方法
        # endregion
        # region # 网页端控制
        # self.WebControl = WebControl()
        # self.WebControl.start()

        # thread = threading.Thread(target=self.run_flask, daemon=True)
        # thread.start()
        # signal_emitter.flaskWeb_signal.connect(self.on_flask_signal)
        # endregion
        # region # 运行时间测试
        # self.thread_testTime = thread_testTime()
        # self.thread_testTime.start()
        self.thread_testTime = QTimer()
        self.thread_testTime.timeout.connect(self.fuc_testTime)
        self.thread_testTime.start(1000)
        # print(globals())
        # endregion
        # region # 初始化参数
        self.headerId_factsheet = 0
        self.headerId_state = 0
        self.headerId_order = 0
        self.orderId = 0
        self.orderState = 'waiting'
        self.start_time = 0
        self.end_time = 0
        # endregion
        # region # 加载配置文件
        # print("当前工作目录:", os.getcwd())
        # if os.name == 'nt':  # Windows系统
        #     os.chdir(r'\Code\mqtt_client')
        #     filename = './config/AgvState.json'
        # elif os.name == 'posix' and virtual_machine==False:  # Linux系统
        #     print("Linux系统")
        #     filename = '/home/sunnybaer/theyn/mqtt_client/config/AgvState.json'
        # with open(filename, 'r') as f:
        #     data = json.load(f)
        global AgvState
        AgvState = AgvParameters.agvState
        self.manufacturer = AgvState['manufacturer']
        self.serialNumber = AgvState['serialNumber']

        # endregion
        # region # 日志管理
        self.sig_print.connect(self.sigCall_print)
        self.btn_clearLog.clicked.connect(self.fuc_clearLog)
        # endregion
        # region # 通讯总线管理

        self.flag_connect_mqtt = False  # 调度系统

        self.flag_connect_handler = False  # 手柄
        self.flag_connect_camera = False  # 导航相机

        self.flag_connect_chasis = False  # 底盘
        self.flag_connect_lifter = False  # 升降机构

        self.flag_connect_IMU = False  # 传感器IMU
        self.flag_connect_motor = False  # 电机

        self.connect_lifter()  # TCP客户端(升降机构)
        self.connect_chasis()  # TCP客户端(底盘)

        self.serial_port = SerialPort(
            port='/dev/ttyUSB0', baudrate=115200, device_type=0x5354)
        self.serial_port.data_received.connect(self.receive_data_handle)
        self.serial_port.start()
        self.deviceStatusChecker = DeviceStatusChecker()  # 设备状态检查器
        self.deviceStatusChecker.sig_reconnect.connect(self.fuc_reconnect)
        # endregion
        # region # MQTT服务器
        self.status_connect = False
        self.flag_send_state = True
        self.btn_server.clicked.connect(self.connect_server)
        self.btn_topic_subscribe.clicked.connect(self.subscribe_topic)
        self.btn_topic_unsubscribe.clicked.connect(self.unsubscribe_topic)
        self.btn_topic_publish.clicked.connect(self.publish_topic)

        self.btn_clearPublish.clicked.connect(self.clearPublish)
        self.btn_test_instantActions.clicked.connect(self.test_instantActions)
        self.btn_test_order.clicked.connect(self.test_order)

        # 带复选框的下拉框
        # ccb=QComboCheckBox()
        # ccb.add_item('选项1')
        # self.horizontalLayout_2.addWidget(ccb)

        # 自动补全到文本输入控件
        wordList = [
            "Apple", "Banana", "Cherry", "Test/Topic1", "Elderberry", "Fig", "Grape", "Honeydew", "hlly", "hlly1", "Ivy gourd", "Jackfruit", "topic",
            "Topic"
        ]
        self.completer = QCompleter(wordList)
        self.completer.setCaseSensitivity(False)
        self.input_topic_name.setCompleter(self.completer)
        # endregion
        # region # TCP服务器(手柄)
        # self.hostip = '10.0.0.217'
        # self.hostip = extract_ip()
        # print(self.hostip)
        # self.server_handler = ServerThread(self.hostip, 6002)
        # self.server_handler.start()
        # self.server_handler.server_log.connect(self.fuc_log)
        # self.server_handler.sig_data_process.connect(self.handler_process)

        self.handler_last = {
            'LM': True,
            'LU': True,
            'LD': True,
            'LL': True,
            'LR': True,
        }

        self.handler = {
            'LM': True,
            'LU': True,
            'LD': True,
            'LL': True,
            'LR': True,
        }
        # endregion
        # region # TCP服务器(相机)
        # self.hostip = '10.0.0.217'
        # self.hostip = extract_ip()
        # self.server_camera = ServerThread(self.hostip, 6005)
        # self.server_camera.start()
        # endregion
        # region # 绘图窗口(显示节点和线信息)
        self.endPoints = []
        self.paint = PaintRoute()  # 运动轨迹窗口
        self.layout_paint.addWidget(self.paint)

        self.paint.scatter.sig_ptData.connect(self.show_ptData)
        self.baseLines = []

        self.last_clickDotIndex = 0
        self.last_clickLineIndex = 0
        self.comboBox_nodeList.activated.connect(self.fuc_showNodeData)
        # self.comboBox_edgeList.currentIndexChanged.connect(self.fuc_showEdgeData)
        self.comboBox_edgeList.activated.connect(self.fuc_showEdgeData)
        # endregion
        # region # 运动模拟
        # self.timer_move = QTimer(self)
        # self.timer_move.timeout.connect(self.fuc_motion_simulation)
        # self.timer_move.start(10)
        # threading.Thread(target=self.fuc_motion_simulation).start()
        self.agv_posX = AgvState['agvPosition']['x']
        self.agv_posY = AgvState['agvPosition']['y']
        self.agv_posX_last = AgvState['agvPosition']['x']
        self.agv_posY_last = AgvState['agvPosition']['y']
        self.agv_theta = AgvState['agvPosition']['theta']
        self.lineEdit_agv_posX.setText(str(self.agv_posX))
        self.lineEdit_agv_posY.setText(str(self.agv_posY))
        self.lineEdit_agv_angle.setText(str(self.agv_theta / 3.14 * 180))
        self.lineSpeed = 0
        self.rateSpeed = 0
        self.movePeriod = 10
        self.multiLineSpeed = 1  # 线速度倍率(提高模拟效率)
        self.multiRateSpeed = 1  # 角速度倍率(提高模拟效率)
        self.creepSpeed = 0.03  # 爬行速度
        self.decDistance = 0  # 提前减速距离
        self.travelDistance_C = 0  # 当前行驶距离
        self.travelDistance_T = 0  # 目标行驶距离
        self.rotateTheta_C = 0  # 当前旋转角度
        self.rotateTheta_T = 0  # 目标旋转角度
        self.node_num_C = 0  # 当前节点编号
        self.node_num_T = 0  # 所有节点个数
        self.edge_num_C = 0  # 当前节点编号
        self.edge_num_T = 0  # 所有节点个数
        self.done_edge = False  # 是否完成当前线段
        self.mode_motion = 'waiting'  # 等待waiting, 直线运动move, 旋转rotate
        self.allow_deviation = 0.07 * self.multiLineSpeed  # 距离误差
        self.btn_forward.clicked.connect(self.fuc_forward)
        self.btn_backward.clicked.connect(self.fuc_backward)
        self.btn_turnLeft.clicked.connect(self.fuc_turnLeft)
        self.btn_turnRight.clicked.connect(self.fuc_turnRight)
        self.btn_stop.clicked.connect(self.fuc_stop)

        self.btn_clear_Trajectory.clicked.connect(self.clear_Trajectory)
        # endregion
        # region # 电机驱动
        self.MotorDrive = MotorDrive()
        self.MotorDrive.start()
        self.MotorDrive.signal_send_canData.connect(self.fuc_send_canData)
        # endregion
        # region # 里程计&速度曲线控制
        # self.Odometer = thread_testTime()
        self.Odometer = Odometer()
        self.Odometer.start()
        self.Odometer.sig_updateAgvPos.connect(self.updateAgvPos)
        self.Odometer.sig_updateAgvSpeed.connect(self.updateAgvSpeed)
        self.Odometer.sig_updateMotionDone.connect(self.updateMotionDone)
        # endregion
        # region # 读码相机
        # self.Odometer.PGV.sig_PGV.connect(self.updatePGV)
        # endregion
        # region # 按键监听线程
        self.keyIsPressed = False
        self.AppIsFocus = False
        # listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release)
        # listener.start()
        # endregion
        # region # 初始化后操作
        # self.connect_server()  # 连接服务器
        # self.MotorDrive.configMotor(0x01)  # 配置电机

        # endregion

    # region # 运行时间测试
    def fuc_testTime(self):
        # print(get_time())
        self.serial_port.write(b'hlly')
        if self.socketClient1.running:
            data = b"hlly1"
            # data=bytes(data)
            self.socketClient1.send_message(data)
        if self.socketClient2.running:
            data = b"hlly2"
            # data=bytes(data)
            self.socketClient2.send_message(data)
    # endregion
    # region # flask网页端控制

    def on_flask_signal(self, message):
        if message == 'stop':
            self.fuc_stop()
        elif message == 'forward':
            self.fuc_forward()
        elif message == 'back':
            self.fuc_backward()
        elif message == 'turnLeft':
            self.fuc_turnLeft()
        elif message == 'turnRight':
            self.fuc_turnRight()

    def run_flask(self):
        # Flask 应用默认运行在 127.0.0.1:5000
        self.app.run(host='0.0.0.0', port=5003, debug=False, threaded=True)

    @app.route('/')
    def index():
        return render_template('index.html')

    @app.route('/stop', methods=['POST'])
    def stop_task():
        signal_emitter.flaskWeb_signal.emit('stop')
        return 'stop'

    @app.route('/forward', methods=['POST'])
    def forward_task():
        signal_emitter.flaskWeb_signal.emit('forward')
        return 'forward'

    @app.route('/back', methods=['POST'])
    def back_task():
        signal_emitter.flaskWeb_signal.emit('back')
        return 'back'

    @app.route('/turnLeft', methods=['POST'])
    def turnLeft_task():
        signal_emitter.flaskWeb_signal.emit('turnLeft')
        return 'turnLeft'

    @app.route('/turnRight', methods=['POST'])
    def turnRight_task():
        signal_emitter.flaskWeb_signal.emit('turnRight')
        return 'turnRight'

    @app.route('/setspeed', methods=['POST'])
    def setspeed_task():
        # signal_emitter.flaskWeb_signal.emit('turnRight')
        data = request.json
        lineSpeed = data.get('lineSpeed', 'Not provided')
        lineSpeed = float(lineSpeed.rstrip('%')) / 100
        print(lineSpeed)
        return 'turnRight'

    # endregion
    # region # 日志管理

    def fuc_log(self, text="", type="", save=False):  # 日志显示区打印
        self.sig_print.emit(text, type, save)

    def sigCall_print(self, text="", classify="", save=False):
        # 定义ANSI转义序列
        red = '\x1b[31m'
        yellow = '\x1b[33m'
        blue = '\x1b[34m'
        reset = '\x1b[0m'  # 重置颜色
        # log = get_time() + "  " + text
        # log=f"{red}{text}{reset}"
        log = f'<span style="color: blue;">{get_time()}</span> {text}'
        self.textBrowser.append(log)
        if save:
            self.fuc_savelog(get_time() + '  ' + text, classify)

    def fuc_clearLog(self, text=""):  # 日志显示区清空
        self.textBrowser.clear()

    def fuc_savelog(self, text, classify):  # 保存日志
        if True:
            dir_name = f'log/{classify}'
            if not os.path.isdir(dir_name):
                os.makedirs(dir_name)
            data_today = datetime.now().strftime('20%y-%m-%d')
            filepath = "{0}/{1}.txt".format(dir_name, data_today)
            with open(filepath, "a+", encoding='utf-8') as f:
                f.write("{}\n".format(text))  # 自带文件关闭功能，不需要再写f.close()

    def fuc_updateState(self, text=""):  # 状态显示区更新
        self.label_state.setText(text)

    # endregion
    # region # 服务器控制（作为服务器）

    def connect_server(self):  # 启动服务器
        if self.status_connect == False:

            self.serverIp = "10.0.0.217"
            # self.serverIp = self.input_ip.text()
            self.serverPort = int(self.input_port.text())
            self.serverID = self.input_clientID.text()
            self.mqtt_thread = MqttThread(
                self.serverIp, self.serverPort, self.serverID)
            # self.mqtt_thread.received_data.connect(self.show_received_data)
            self.mqtt_thread.server_log.connect(self.fuc_log)
            self.mqtt_thread.sig_connect_feedback.connect(
                self.connect_server_feedback)
            self.mqtt_thread.sig_data_analysis.connect(self.analysis_data)
            try:
                self.mqtt_thread.start()
                self.btn_server.setText("断开服务器")
                self.status_connect = True
            except Exception as e:
                self.fuc_log(f"连接服务器失败，原因：{e}")
        else:
            self.btn_server.setText("连接服务器")
            self.status_connect = False
            self.mqtt_thread.stop()
            self.timer_heartbeat.stop()
            self.com_topic_list.clear()

    def connect_server_feedback(self, status):
        if status:
            print("连接服务器成功")
            # 开启定时心跳
            self.timer_heartbeat = QTimer(self)
            self.timer_heartbeat.timeout.connect(self.send_heartbeat)
            self.timer_heartbeat.start(100)

            # 订阅默认话题
            self.subscribe(f'Test/Topic1')
            self.subscribe(
                f'robot/v2/{self.manufacturer}/{self.serialNumber}/instantActions')
            self.subscribe(
                f'robot/v2/{self.manufacturer}/{self.serialNumber}/order')
            self.subscribe(
                f'robot/v2/{self.manufacturer}/{self.serialNumber}/factsheet')
            # self.subscribe(f'robot/v2/{self.manufacturer}/{self.serialNumber}/connection')
            # self.subscribe(f'robot/v2/{self.manufacturer}/{self.serialNumber}/state')
            topic = f'robot/v2/{self.manufacturer}/{self.serialNumber}/connect'
            content = AgvParameters.connection
            content['headerId'] = 1
            content['timestamp'] = get_timestamp()
            content = json.dumps(content)
            self.publish(topic, content)
        else:
            self.btn_server.setText("连接服务器")
            self.status_connect = False
            self.mqtt_thread.stop()
            print("连接服务器失败")
            time.sleep(3)
            if self.checkBox_mode_manual.isChecked():
                return
            # self.connect_server()  #重新连接

    def send_heartbeat(self):
        if self.flag_send_state:
            topic = f'robot/v2/{self.manufacturer}/{self.serialNumber}/state'
            self.headerId_state = update_headerId(self.headerId_state)
            AgvState['headerId'] = self.headerId_state
            AgvState['timestamp'] = get_timestamp()
            content = json.dumps(AgvState)
            self.publish(topic, content)
        self.flag_send_state = True

    def subscribe(self, topic):
        if self.status_connect:
            if topic not in [self.com_topic_list.itemText(i) for i in range(self.com_topic_list.count())]:
                self.com_topic_list.addItem(topic)
                self.mqtt_thread.subscribe(topic)
            else:
                self.fuc_log(f"已经订阅话题'{topic}' 。")
        else:
            self.fuc_log("订阅默认话题失败")

    def subscribe_topic(self):
        if self.status_connect:
            topic = self.input_topic_name.text()

            if topic not in [self.com_topic_list.itemText(i) for i in range(self.com_topic_list.count())]:
                self.com_topic_list.addItem(topic)
                self.mqtt_thread.subscribe(topic)
            else:
                self.fuc_log(f"已经订阅话题'{topic}' 。")
        else:
            self.fuc_log("请先连接服务器")

    def unsubscribe_topic(self):
        if self.status_connect:
            topic = self.com_topic_list.currentText()
            self.mqtt_thread.unsubscribe(topic)
            self.com_topic_list.removeItem(self.com_topic_list.findText(topic))
        else:
            self.fuc_log("请先连接服务器")

    def publish(self, topic, content):
        isShow = self.check_show_publish.isChecked()
        self.mqtt_thread.publish(topic, content, isShow)

    def publish_topic(self):
        if self.status_connect:
            topic = self.input_topic_name.text()
            data = self.text_topic_content.toPlainText()
            isShow = self.check_show_publish.isChecked()
            self.mqtt_thread.publish(topic, data, isShow)
        else:
            self.fuc_log("请先连接服务器")

    def clearPublish(self):
        self.text_topic_content.clear()

    def test_instantActions(self):
        self.input_topic_name.setText(
            f'robot/v2/{self.manufacturer}/{self.serialNumber}/instantActions')
        content = AgvParameters.testInstantActions
        self.text_topic_content.setText(json.dumps(content))
        self.publish_topic()

    def test_order(self):
        self.input_topic_name.setText(
            f'robot/v2/{self.manufacturer}/{self.serialNumber}/order')
        content = AgvParameters.testOrder
        self.text_topic_content.setText(json.dumps(content))
        self.publish_topic()

    def analysis_data(self, topic, json_str):
        if self.check_show_json.isChecked():
            try:
                # print('收到数据')
                # json_str = '{"name": "John", "age": 30, "city": "New York"}'
                data = json.loads(json_str)
                # self.fuc_log(f"成功解析 {topic} 的数据")
                # str_data=''
                # for key, value in data.items():
                #     str_data += f"{key}: {value}"+"  "
                # self.fuc_log(str_data)
                if topic == f'robot/v2/{self.manufacturer}/{self.serialNumber}/instantActions':
                    self.process_instantActions(data)
                elif topic == f'robot/v2/{self.manufacturer}/{self.serialNumber}/order':
                    self.process_order(data)
                else:
                    self.fuc_log("其它话题数据")
                    print(f"未处理的数据{topic} {data}")
            except json.JSONDecodeError as e:
                self.fuc_log(f"JSON解析错误: {e}")
                # print(type(json_str))
                print(json_str)

    # endregion
    # region # 调度数据处理

    def process_instantActions(self, data):
        try:
            print('hlly')
            # print(data['actions'])
            self.input_actionId.setText(data['actions'][0]['actionId'])
            self.input_actionDescription.setText(
                data['actions'][0]["actionDescription"])
            self.input_actionType.setText(data['actions'][0]["actionType"])
            self.input_actionParameters.setText(
                str(data['actions'][0]["actionParameters"]))
            if self.stackedWidget_nodeEdge.currentIndex() != 2:
                self.stackedWidget_nodeEdge.setCurrentIndex(2)
            self.reply_factsheet()
        except Exception as e:
            print(f"数据处理异常{e}")

    def process_order_debug(self, data):
        print('process_order')

    def process_order(self, data):
        # region #处理新消息和新订单
        headerId_order = data['headerId']
        orderId = data['orderId']
        if self.headerId_order == headerId_order:
            # print(f'收到重复订单消息{headerId_order}')
            return
        self.headerId_order = headerId_order
        if self.orderId == orderId:
            # print(f'收到重复订单{self.orderId}')
            return
        self.orderId = orderId
        print(f'process_order{self.orderId}')
        AgvState['orderId'] = self.orderId
        if self.orderState == 'running':
            print('订单执行中')
            return
        self.orderState = 'running'
        # endregion
        # region #绘制地图中点和线
        self.paint.dotdata = []  # 清空点
        self.endPoints = []
        for baseLine in self.baseLines:  # 清空线
            self.paint.removeItem(baseLine)
        self.baseLines = []

        nodes = data['nodes']
        for node in nodes:
            nodeId = node['nodeId']
            nodePosition = node['nodePosition']
            newpoint = {
                'pos': (nodePosition['x'], nodePosition['y']), 'data': node}
            self.paint.dotdata.append(newpoint)
            self.endPoints.append(
                {'nodeId': nodeId, 'x': nodePosition['x'], 'y': nodePosition['y']})
        self.paint.refresh_dot_order()

        edges = data['edges']
        for index, edge in enumerate(edges):
            edgeId = edge['edgeId']
            startNodeId = edge['startNodeId']
            endNodeId = edge['endNodeId']
            for node in self.endPoints:
                if node['nodeId'] == startNodeId:
                    x1 = node['x']
                    y1 = node['y']
                    break
            for node in self.endPoints:
                if node['nodeId'] == endNodeId:
                    x2 = node['x']
                    y2 = node['y']
                    break
            baseLine = LineSegmentROI(
                positions=[[x1, y1], [x2, y2]], data=edge)
            baseLine.sig_lineData.connect(self.show_lineData)
            self.paint.addItem(baseLine)
            self.baseLines.append(baseLine)
        # endregion
        # region #更新点和线列表
        self.comboBox_nodeList.clear()
        self.nodeList = []
        for index, node in enumerate(nodes):
            self.nodeList.append(node)
            self.comboBox_nodeList.addItem(str(index))
        self.comboBox_edgeList.clear()
        self.edgeList = []
        for index, edge in enumerate(edges):
            self.edgeList.append(edge)
            self.comboBox_edgeList.addItem(str(index))

        # endregion
        # region #执行命令
        AgvState['nodeStates'] = nodes
        AgvState['edgeStates'] = edges
        threading.Thread(target=self.traverseNodeAndEdge,
                         args=(self.nodeList, self.edgeList)).start()
        # endregion

    # endregion
    # region # 回复请求

    def reply_factsheet(self):
        topic = f'robot/v2/{self.manufacturer}/{self.serialNumber}/factsheet'
        content = AgvParameters.factsheet
        self.headerId_factsheet = update_headerId(self.headerId_factsheet)
        content['headerId'] = self.headerId_factsheet
        content['timestamp'] = get_timestamp()
        content = json.dumps(content)
        self.publish(topic, content)

    # endregion
    # region # 绘图窗口(显示节点和线信息)
    def show_ptData(self, data, data1):
        # print(data)
        if self.stackedWidget_nodeEdge.currentIndex() != 0:
            self.stackedWidget_nodeEdge.setCurrentIndex(0)
        self.lineEdit_nodeId.setText(data['nodeId'])
        self.lineEdit_sequenceId.setText(str(data['sequenceId']))
        self.lineEdit_nodeDescription.setText(data['nodeDescription'])
        self.lineEdit_released.setText(str(data['released']))

        self.lineEdit_nodePosition.setText(str(data['nodePosition']))
        self.lineEdit_nodePosition_x.setText(str(data['nodePosition']['x']))
        self.lineEdit_nodePosition_y.setText(str(data['nodePosition']['y']))
        self.lineEdit_nodePosition_theta.setText(
            str(data['nodePosition']['theta']))

        self.lineEdit_actions.setText(str(data['actions']))

        if data1:
            data = data1
            self.lineEdit_nodeId_2.setText(data['nodeId'])
            self.lineEdit_sequenceId_2.setText(str(data['sequenceId']))
            self.lineEdit_nodeDescription_2.setText(data['nodeDescription'])
            self.lineEdit_released_2.setText(str(data['released']))

            self.lineEdit_nodePosition_2.setText(str(data['nodePosition']))
            self.lineEdit_nodePosition_x_2.setText(
                str(data['nodePosition']['x']))
            self.lineEdit_nodePosition_y_2.setText(
                str(data['nodePosition']['y']))
            self.lineEdit_nodePosition_theta_2.setText(
                str(data['nodePosition']['theta']))

            self.lineEdit_actions_2.setText(str(data['actions']))
        else:
            data = {
                'nodeId': '',
                'sequenceId': '',
                'nodeDescription': '',
                'released': '',
                'nodePosition': {
                    'x': '',
                    'y': '',
                    'theta': ''
                },
                'actions': []
            }
            self.lineEdit_nodeId_2.setText(data['nodeId'])
            self.lineEdit_sequenceId_2.setText(str(data['sequenceId']))
            self.lineEdit_nodeDescription_2.setText(data['nodeDescription'])
            self.lineEdit_released_2.setText(str(data['released']))

            self.lineEdit_nodePosition_2.setText('')
            self.lineEdit_nodePosition_x_2.setText(
                str(data['nodePosition']['x']))
            self.lineEdit_nodePosition_y_2.setText(
                str(data['nodePosition']['y']))
            self.lineEdit_nodePosition_theta_2.setText(
                str(data['nodePosition']['theta']))

            self.lineEdit_actions_2.setText('')

    def show_lineData(self, data):
        if self.stackedWidget_nodeEdge.currentIndex() != 1:
            self.stackedWidget_nodeEdge.setCurrentIndex(1)
        if 'edgeId' in data:
            self.lineEdit_edgeId.setText(data['edgeId'])
        if 'sequenceId' in data:
            self.lineEdit_sequenceId_3.setText(str(data['sequenceId']))
        if 'edgeDescription' in data:
            self.lineEdit_edgeDescription.setText(data['edgeDescription'])
        if 'released' in data:
            self.lineEdit_released_3.setText(str(data['released']))
        if 'startNodeId' in data:
            self.lineEdit_startNodeId.setText(data['startNodeId'])
        if 'endNodeId' in data:
            self.lineEdit_endNodeId.setText(str(data['endNodeId']))
        if 'maxSpeed' in data:
            self.lineEdit_maxSpeed.setText(str(data['maxSpeed']))
        if 'maxHeight' in data:
            self.lineEdit_maxHeight.setText(str(data['maxHeight']))
        if 'minHeight' in data:
            self.lineEdit_minHeight.setText(str(data['minHeight']))
        if 'orientation' in data:
            self.lineEdit_orientation.setText(str(data['orientation']))
        if 'orientationType' in data:
            self.lineEdit_orientationType.setText(str(data['orientationType']))
        if 'direction' in data:
            self.lineEdit_direction.setText(str(data['direction']))
        if 'rotationAllowed' in data:
            self.lineEdit_rotationAllowed.setText(str(data['rotationAllowed']))
        if 'maxRotationSpeed' in data:
            self.lineEdit_maxRotationSpeed.setText(
                str(data['maxRotationSpeed']))
        if 'trajectory' in data:
            self.lineEdit_trajectory.setText(str(data['trajectory']))
        if 'length' in data:
            self.lineEdit_length.setText(str(data['length']))
        if 'actions' in data:
            self.lineEdit_actions_3.setText(str(data['actions']))

    def fuc_showNodeData(self, index):
        self.show_ptData(self.nodeList[index], {})
        # 点击列表中的点和线，地图中的点颜色更改
        self.paint.dotdata[self.last_clickDotIndex]['brush'] = 'b'
        self.paint.dotdata[self.last_clickDotIndex]['pen'] = 'b'
        self.paint.dotdata[index]['brush'] = 'r'
        self.paint.dotdata[index]['pen'] = 'r'
        self.paint.refresh_dot_order()
        self.last_clickDotIndex = index

    def fuc_showEdgeData(self, index):
        self.show_lineData(self.edgeList[index])
        # 点击列表中的点和线，地图中的线颜色更改
        color = (0, 0, 255, 155)
        pen = pg.mkPen(color=color, width=7)
        self.baseLines[self.last_clickLineIndex].setPen(pen)
        hovercolor = (255, 0, 0, 100)
        pen = pg.mkPen(color=hovercolor, width=7)
        self.baseLines[index].setPen(pen)
        self.last_clickLineIndex = index

    # endregion
    # region # 通讯总线管理
    def fuc_reconnect(self, device):
        if device == 'chasis':
            if self.socketClient1.running:
                pass
                # self.socketClient1.stop()#超时收不到数据，自动断开
            else:
                # self.connect_chasis()
                pass
        # elif device == 'lifter':
        #     if self.socketClient2.running:
        #         pass
        #     else:
        #         self.connect_lifter()

    def connect_lifter(self):
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.settimeout(3)
        # s.settimeout(None)
        # s_ip = '10.0.0.217'
        # s_port = 5007
        s_ip = '192.168.16.11'
        s_port = 7007
        try:
            s.connect((s_ip, s_port))
            self.fuc_log(str(s.getpeername()) + "连接成功")
        except Exception as e:
            self.fuc_log(s_ip + ": " + str(s_port) +
                         " " + str(e), 'connect', True)
        self.socketClient2 = clientThread(0x4354, s)
        self.socketClient2.log.connect(self.fuc_log)
        self.socketClient2.data_received.connect(self.receive_data_handle)
        self.socketClient2.start()

    def connect_chasis(self):
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.settimeout(3)  # 超时没有收到数据，自动断开
        # s_ip = '10.0.0.217'
        # s_port = 5007
        s_ip = '192.168.16.10'
        s_port = 5007
        try:
            s.connect((s_ip, s_port))
            # s.settimeout(None)
            self.fuc_log(str(s.getpeername()) + "连接成功")
        # except socket.error as e:
        except Exception as e:
            self.fuc_log(s_ip + ": " + str(s_port) +
                         " " + str(e), 'connect', True)
        self.socketClient1 = clientThread(0x4357, s)
        self.socketClient1.log.connect(self.fuc_log)
        self.socketClient1.data_received.connect(self.receive_data_handle)
        self.socketClient1.disConnect.connect(self.servers_disConnect)
        self.socketClient1.start()

    def servers_disConnect(self, device, thread):
        self.deviceStatusChecker.status_chasis = 0
        print(device)
        print(thread)
    # endregion
    # region # TCP服务器(手柄)

    def handler_process(self, data):
        # 帧头、帧尾、长度都对
        if ((len(data) > 8) & (len(data) == data[3]) & (data[len(data) - 1] == 0xda)):
            self.handler_apply(data[4:len(data) - 2])

    def handler_apply(self, buf):
        self.handler['LU'] = (buf[4] & 0x80)
        self.handler['LD'] = (buf[4] & 0x40)
        self.handler['LL'] = (buf[4] & 0x20)
        self.handler['LR'] = (buf[4] & 0x10)
        self.handler['LM'] = (buf[4] & 0x08)
        self.handler['L1'] = (buf[4] & 0x04)

        self.handler['RU'] = (buf[5] & 0x80)
        self.handler['RD'] = (buf[5] & 0x40)
        self.handler['RL'] = (buf[5] & 0x20)
        self.handler['RR'] = (buf[5] & 0x10)
        self.handler['RM'] = (buf[5] & 0x08)
        self.handler['R1'] = (buf[5] & 0x04)

        # 按键按下
        if self.handler_last['LM'] and not self.handler['LM']:
            self.fuc_stop()
        elif self.handler_last['LU'] and not self.handler['LU']:
            self.fuc_forward()
        elif self.handler_last['LD'] and not self.handler['LD']:
            self.fuc_backward()
        elif self.handler_last['LL'] and not self.handler['LL']:
            self.fuc_turnLeft()
        elif self.handler_last['LR'] and not self.handler['LR']:
            self.fuc_turnRight()

        # 按键松开
        if (not self.handler_last['LU'] and self.handler['LU']) or \
           (not self.handler_last['LD'] and self.handler['LD']) or \
           (not self.handler_last['LL'] and self.handler['LL']) or \
           (not self.handler_last['LR'] and self.handler['LR']):
            self.fuc_stop()
        self.handler_last = self.handler.copy()

    # endregion
    # region # TCP客户端数据处理
    def receive_data_handle(self, identify, device_type, data):  # 接收数据处理
        self.receive_data_restore(identify, device_type, data)

    def receive_data_restore(self, identify, device_type, data):  # 接收数据还原（去转义）
        pattern = rb'(?<!\\);'  # 如果';'前为'\'则不进行分割
        datalist = [x for x in re.split(pattern, data) if x]  # 去掉空''
        for i in datalist:
            pattern = rb'[\\](?=;)'
            i = re.sub(pattern, b"", i)  # 如果'\'后面是';'去掉'\'
            if len(i) > 1:
                # print(len(data))
                self.receive_data_classify(identify, device_type, i)
            else:
                print(data)

    def receive_data_classify(self, identify, device_type, data):  # 接收数据分类
        if device_type == 0x4354:
            self.deviceStatusChecker.time_lifter = QTime.currentTime()
        elif device_type == 0x4357:
            self.deviceStatusChecker.time_chasis = QTime.currentTime()
        if (data[0] == 0x50) or (data[0] == 0x43):  # 接收十六进制数组
            a = HexStrAddSpace(data.hex())
            self.fuc_log(str(identify) + a, 'data', True)
        elif ((data[0] == 0x42) & (data[1] == 0x44)):  # BD（0x42,0x44）来自于控制板的数据
            self.receive_data_verify(identify, device_type, data)
        else:  # 接收中文或英文或其它
            try:
                a = data.decode('utf-8')
                print("utf-8")
            except Exception:
                try:
                    a = data.decode('gbk')
                    print("gbk")
                except:
                    a = str(data)
                    print("字符串")
            self.fuc_log(str(identify) + " " + a)
        # except:
        #     self.fuc_log("接收数据分类错误")

    def receive_data_verify(self, identify, device_type, data):  # 接收数据验证
        # print(len(data))
        # if(len(data)>3):
        try:
            # 帧头、帧尾、长度都对
            if ((len(data) > 8) & (len(data) == data[3]) & (data[len(data) - 1] == 0xda)):
                # print("单帧")
                self.receive_data_apply(
                    identify, device_type, data[4:len(data) - 2])
                # a=HexStrAddSpace(data.hex())#查看原始数据
                # self.fuc_log(str(identify)+a)
            elif ((len(data) < 300) & (len(data) > data[3])):  # 多帧数据拆分开处理
                processData = data[:data[3]]
                # self.fuc_data_verify(processData) #!!!数据合格分开后，重新验证
                # !!!数据合格分开后，重新验证，不能直接调用fuc_data_apply！！！
                self.receive_data_verify(identify, device_type, processData)
                remainData = data[data[3]:]
                # print("多帧")
                # print("总长度"+str(len(data))+'长度'+str(data[3]))
                self.receive_data_classify(identify, device_type, remainData)
                # print("剩余长度"+str(len(remainData)))
            else:
                if (data[len(data) - 1] != 0xda):
                    # self.fuc_log(str(identify)+"帧尾错误")
                    try:
                        print(data)
                        a = data.decode('utf-8')
                        print("english")
                    except Exception:
                        try:
                            # a=data.decode('gbk')
                            a = HexStrAddSpace(data.hex())
                            print("中文")
                        except:
                            a = str(data)
                    # self.fuc_log(str(identify)+HexStrAddSpace(data.hex()))
                    self.fuc_log(str(identify) + a)
                else:
                    print("数据格式错误")
                    # self.fuc_data_apply(data[4:len(data)-2])
        except:
            a = HexStrAddSpace(data.hex())
            self.fuc_log(str(identify) + a)

    def receive_data_apply(self, identify, device_type, data):  # 接收数据应用
        if ((data[0] == 0x00) & (data[1] == 0x00)):  # 升级进度
            data = data[2:]
            if data[0] == 0xff:
                self.fuc_log(str(identify) + "升级失败")
            else:
                lenth = data[0] << 24 | data[1] << 16 | data[2] << 8 | data[3]
                try:
                    self.servers_pgBars[device_type].setValue(lenth)
                except Exception:
                    self.fuc_log(str(identify) + "返回数据错误")
                if self.fileLen == lenth:
                    self.fuc_log(str(identify) + "升级完成")

    # endregion
    # region # 运动模拟
    # region # 按钮控制运动
    def fuc_forward(self):
        self.Odometer.lineSpeed_T = abs(
            float(self.lineEdit_lineSpeed_T.text()))
        self.Odometer.rateSpeed_T = 0
        self.Odometer.flag_controlSpeed = 1

    def fuc_backward(self):
        self.Odometer.lineSpeed_T = - \
            abs(float(self.lineEdit_lineSpeed_T.text()))
        self.Odometer.rateSpeed_T = 0
        self.Odometer.flag_controlSpeed = 1

    def fuc_turnLeft(self):
        self.Odometer.lineSpeed_T = 0
        self.Odometer.rateSpeed_T = abs(
            float(self.lineEdit_rateSpeed_T.text()))
        self.Odometer.flag_controlSpeed = 1

    def fuc_turnRight(self):
        self.Odometer.lineSpeed_T = 0
        self.Odometer.rateSpeed_T = - \
            abs(float(self.lineEdit_rateSpeed_T.text()))
        self.Odometer.flag_controlSpeed = 1

    def fuc_stop(self):
        self.Odometer.lineSpeed_T = 0
        self.Odometer.rateSpeed_T = 0
        self.Odometer.flag_controlSpeed = 1

        # endregion
    def clear_Trajectory(self):
        self.paint.agvTrajectoryDot = []
        self.paint.agvTrajectory.setData(self.paint.agvTrajectoryDot)

    def calculate_distance(self, x1, y1, x2, y2):
        distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
        return distance

    # endregion
    # region # 遍历节点和边
    def traverseNodeAndEdge(self, nodeList, edgeList):
        self.node_num_C = 0
        self.node_num_T = len(self.nodeList)
        self.edge_num_C = 0
        self.edge_num_T = len(self.edgeList)
        print('遍历节点和边，节点数量:%d,边数量:%d' % (self.node_num_T, self.edge_num_T))
        self.execute_node(0)

    def execute_node(self, index):
        self.node_num_C = index
        if self.nodeList[index]['released'] == False:
            print('节点%d未释放' % index)
            return
        # else:
        #     print('节点%d已释放' % index)
        x1 = self.nodeList[index]['nodePosition']['x']
        x2 = self.agv_posX
        y1 = self.nodeList[index]['nodePosition']['y']
        y2 = self.agv_posY
        distance = self.calculate_distance(x1, y1, x2, y2)
        if distance < self.allow_deviation:
            print('AGV遍历节点%d完成' % index)
        else:
            print('AGV遍历节点%d失败,偏差距离为%.3f' % (index, distance))
            self.Odometer.rateSpeed_T = 0
            self.Odometer.lineSpeed_T = 0
            self.Odometer.flag_controlSpeed = 1
            self.orderState = 'error'
            return
        # 节点执行完毕，更新节点状态
        AgvState['nodeStates'].pop(0)
        AgvState['lastNodeId'] = self.nodeList[index]['nodeId']
        AgvState['lastNodeSequenceId'] = self.nodeList[index]['sequenceId']
        self.send_heartbeat()
        self.flag_send_state = False

        # 执行下条边
        if index < self.edge_num_T:
            self.edge_num_C = index
            self.execute_edge(index)
        else:
            self.orderState = 'finish'
            print('遍历节点和边完成')

    def execute_edge(self, index):
        if self.edgeList[index]['released'] == False:
            print('边%d未释放' % index)
            return
        # else:
        #     print('边%d已释放' % index)
        print('执行边%d' % index)
        # region #边上旋转运动
        if (self.edgeList[index]['startNodeId'] == self.edgeList[index]['endNodeId']
                or (self.nodeList[index]['nodePosition']['x'] == self.nodeList[index + 1]['nodePosition']['x']
                    and self.nodeList[index]['nodePosition']['y'] == self.nodeList[index + 1]['nodePosition']['y'])):
            self.Odometer.lineSpeed_T = 0
            self.travelDistance_C = 0
            self.travelDistance_T = 0
            self.rotateTheta_C = 0
            agv_deviationAngle = self.nodeList[index +
                                               1]['nodePosition']['theta'] - self.agv_theta
            agv_deviationAngle = (agv_deviationAngle +
                                  math.pi) % (2 * math.pi) - math.pi
            # 目标角速度
            if agv_deviationAngle > 0:
                self.Odometer.rateSpeed_T = 0.3
            else:
                self.Odometer.rateSpeed_T = -0.3
            # 目标角度
            self.Odometer.rotateTheta_T = self.nodeList[index +
                                                        1]['nodePosition']['theta']
            self.Odometer.planTheta_last = abs(agv_deviationAngle)
            # 减速距离
            speed = 0.3
            self.Odometer.decTheta = 0.7 * speed * speed / \
                self.Odometer.agv_RateDeceleration + self.Odometer.creepTheta  # 0.7为经验值,理论上为0.5
            self.Odometer.mode_motion = 'rotateToNode'
            print('旋转，当前角度:%.3f,目标角度:%.3f,偏差角度:%.3f' %
                  (AgvState['agvPosition']['theta'], self.nodeList[index + 1]['nodePosition']['theta'], agv_deviationAngle))
        # endregion
        # region #边上直线运动
        else:
            self.Odometer.rateSpeed_T = 0
            self.rotateTheta_C = 0
            self.rotateTheta_T = 0
            self.travelDistance_C = 0

            # 目标点距离
            x1 = self.agv_posX
            y1 = self.agv_posY
            x2 = self.nodeList[index + 1]['nodePosition']['x']
            y2 = self.nodeList[index + 1]['nodePosition']['y']
            self.Odometer.planDistance_last = self.Odometer.calculate_distance(
                x1, y1, x2, y2)
            # 路径方向
            x3 = self.nodeList[index]['nodePosition']['x']
            y3 = self.nodeList[index]['nodePosition']['y']
            self.Odometer.startNode_x = x3
            self.Odometer.startNode_y = y3
            self.Odometer.targetNode_x = x2
            self.Odometer.targetNode_y = y2
            path_direction = math.atan2(y2 - y3, x2 - x3) * 180 / math.pi
            path_direction = angle_rectify(path_direction)
            self.Odometer.path_direction = path_direction
            # 目标速度
            self.Odometer.lineSpeed_T = 0.3
            car_degrees = self.agv_theta * (180 / math.pi)
            car_degrees = angle_rectify(car_degrees)
            angle_error = path_direction - car_degrees
            angle_error = angle_rectify(angle_error)
            if abs(angle_error) > 90:
                flag_car_back = True
            else:
                flag_car_back = False
            # print('path_direction:', path_direction)
            # print('car_degrees:', car_degrees)
            if flag_car_back:  # 倒车
                self.Odometer.lineSpeed_T = -0.3
            # 减速距离
            speed = 0.3
            self.Odometer.decDistance = 0.7 * speed * speed / \
                self.Odometer.agv_deceleration + self.Odometer.creepDistance  # 0.7为经验值,理论上为0.5
            self.Odometer.mode_motion = 'moveToNode'
            print(
                f'前进,距离目标点{self.Odometer.planDistance_last:.3f}米,减速距离{self.Odometer.decDistance:.3f}米')

            # 判断是否连续运动
            self.Odometer.flag_enSlowDown = True
            if (index + 1) < self.edge_num_T:
                if self.edgeList[index + 1]['startNodeId'] != self.edgeList[index + 1]['endNodeId']:
                    self.Odometer.flag_enSlowDown = False
        # endregion
        self.Odometer.flag_controlSpeed = 1
        self.done_edge = False

    # endregion
    # region # 里程计&速度曲线控制
    def updateAgvPos(self, agv_posX, agv_posY, agv_theta):
        # region # 更新AGV位置信息
        # 用于判断距节点距离
        self.agv_posX = agv_posX
        self.agv_posY = agv_posY
        self.agv_theta = agv_theta
        # 可视化显示
        self.lineEdit_agv_posX.setText(str(round(agv_posX, 3)))
        self.lineEdit_agv_posY.setText(str(round(agv_posY, 3)))
        degrees = agv_theta * (180 / math.pi)
        self.lineEdit_agv_angle.setText(str(round(degrees, 1)))
        # 上报调度信息
        AgvState['agvPosition']['x'] = agv_posX
        AgvState['agvPosition']['y'] = agv_posY
        AgvState['agvPosition']['theta'] = agv_theta
        # endregion
        # region # 更新AGV地图位置
        self.agvPos = [{'pos': (agv_posX, agv_posY)}]
        self.paint.agvModel.setData(self.agvPos)
        # endregion
        # region # 更新AGV运动轨迹
        if self.check_show_Trajectory.isChecked():
            # print(len(self.paint.agvTrajectoryDot))
            if agv_posX != self.agv_posX_last or agv_posY != self.agv_posY_last:
                self.agv_posX_last = agv_posX
                self.agv_posY_last = agv_posY
                self.paint.agvTrajectoryDot.append(
                    {'pos': (agv_posX, agv_posY)})
                self.paint.refresh_dot_agvTrajectoryDot()
        # endregion

    def updateAgvSpeed(self, lineSpeed, rateSpeed):
        self.MotorDrive.control_motor(lineSpeed, rateSpeed)
        self.lineEdit_lineSpeed_C.setText(str(lineSpeed))
        self.lineEdit_rateSpeed_C.setText(str(rateSpeed))
        # AgvState['velocity']['x'] = self.lineSpeed
        # AgvState['velocity']['y'] = self.lineSpeed

    def updateMotionDone(self):
        self.done_edge = True
        # 等待完全停止
        if self.Odometer.flag_enSlowDown:
            while self.Odometer.flag_controlSpeed == 1:
                self.Odometer.rateSpeed_C = 0
                self.Odometer.lineSpeed_C = 0
                pass
        # 边执行完毕，更新边状态
        print('AGV遍历边%d完成' % self.edge_num_C)
        AgvState['edgeStates'].pop(0)
        # 执行下个点
        self.execute_node(self.edge_num_C + 1)

    def fuc_send_canData(self, data):
        # if self.socketClient1:
        #     self.socketClient1.send_message(data)
        # print(data)
        pass

    # endregion
    # region # 读码相机

    def updatePGV(self, tag_id, tag_posX, tag_posY, tag_theta):
        self.lineEdit_PGV_Num.setText(str(tag_id))
        self.lineEdit_PGV_X.setText(str(tag_posX))
        self.lineEdit_PGV_Y.setText(str(tag_posY))
        self.lineEdit_PGV_YAW.setText(str(tag_theta))

    # endregion
    # region # 定义按键监听回调函数
    def on_press(self, key):
        if self.AppIsFocus == True:
            if self.keyIsPressed == False:
                self.keyIsPressed = True
                # print("键盘按下")
                try:
                    if key.char == 'w':
                        self.fuc_forward()
                    elif key.char == 's':
                        self.fuc_backward()
                    elif key.char == 'a':
                        self.fuc_turnLeft()
                    elif key.char == 'd':
                        self.fuc_turnRight()

                except AttributeError:
                    if key == keyboard.Key.space:
                        self.fuc_stop()

    def on_release(self, key):
        if self.AppIsFocus == True:
            self.keyIsPressed = False
            # print('按键松开')

    def focusInEvent(self, event):  # 聚焦时开启键盘监听，快捷键控制AGV运动
        self.AppIsFocus = True
        # print("聚焦")

    def focusOutEvent(self, event):  # 失焦时关闭键盘监听,防止快捷键误触发AGV运动
        self.AppIsFocus = False
        # print("失焦")

    # endregion


if __name__ == '__main__':
    app = QApplication(sys.argv)
    signal_emitter = SignalEmitter()
    main_window = Window_main()
    main_window.show()
    sys.exit(app.exec_())
